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ABSTRACT 

Optimal estimation techniques are manipulated to demonstrate the 
feasibility of using a Wiener filter for trim analysis of a submarine. 

The essence of the problem is the estimation of deviations of the sub- 
marine's weight and longitudinal center of gravity from those values 
which enable the submarine to maintain ordered depth and angle of 
pitch with minimal control surface deflections. 

Linearized equations of motion are developed and put into state 
vector notation. The state vector itself contains the variables which 
are to be estimated. Optimal control techniques are used to design a 
linear regulator which is used to control the simulated submarine in 
the vertical plane. Two types of filters are designed. The first is 
sub-optimal in that its gains are derived by pole placement techniques 
which are used to ensure filter stability. The second filter is a 
Wiener filter, in that its gains come from the steady state solution 
of the matrix Riccati equation. A least squares smoother is designed to 
smooth filtered estimates of the variables of interest. 

Simulations on a digital computer are used to demonstrate the 
ability of the filters to estimate the submarine's state of trim in the 
presence of disturbances to the submarine and the measurement instru- 
ments. It is also demonstrated that the Wiener filter is capable of 
estimating the severity of a flooding casualty on the submarine. 
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1. General 

Over the past few decades, many technological advances have been 
made in the design and construction of submarines. They are traveling 
faster, deeper, and for longer periods of time than ever before. 
Nontheless, there are many things that remain the same. Naturally, 
the physical laws governing the behavior of submarines in hydrospace 
have not been altered by man's more adept manipulations. To be 
specific submarines are still subjected to hydrostatic and hydrodynamic 
forces, the magnitude and direction of which are dictated by the design- 
er's appreciation for the natural law:-.« Another unaltered fact is that 
these undersea craft are manned by sumbariners whose fates lie largely 
in their respect for the sea that surrounds them Tragedies of the past 
bare stark evidence to how intolerant of error the sea can be. Yet it 
is said, ’‘To err is human.” Designers, in their appreciation of this 
fact, strive for safety through factors of safety and various devices 
designed to protect the submarine and her crew. Submariner’s seek 
refuge in exhaustive training, practice, and vigilance Indeed, the 
results have been impressive. It has been the experience of the Author 
and most submariners that serious mishaps are seldom precipitated 
by any one mistake or mechanical failure. Post accident investigations 
most often reveal a chain of events which led to a disturbing, if not 
disasterous, culmination. 

Imagine, for example, a nuclear submarine making a high speed 
submerged transit. It has been going fast for several hours, and un- 
known to the crew, it has become grossly heavy, or out of trim as 
submariner's are apt to say. The pitch angle of the boat and the control 
surface deflections have been monitored closely, but their slight 
deviations from neutral angles and the ensuing large hydrodynamic 
forces remain undetected. Suddenly, main propulsion is lost, and 
emergency power is channeled to auxiliary systems while the submarine 
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begins to slow and the engineering department works hurridly to 
restore main power. At the diving stand, it soon becomes apparent 
that much greater deflections of the control surfaces and pitch angle 
are required to maintain depth. The diving officer, realizing that the 
boat is heavy, gives orders to commence pumping variable ballast 
to sea. The officer of the deck quickly grasps the gravity of the situ- 
ation and orders an emergency surfacing. The control surfaces go 
to maximum deflections in an effort to drive the boat toward the surface 
while air valves are opened to blow main ballast tanks. Unexpectedly 
the air lines rupture and emergency power is lost. In the dim light 
of the battle lanterns panic begins to take hold as the men sense the 
rising pressure of the atmosphere against their eardrums. Having 
lost its initial momentum, the submarine reaches an apex and then 
begins to descend, slowly at first but then more and more rapidly. 

The officer of the deck has had difficulty making himself understood, 
but finally the main ballast blow is secured and the air and trim systems 
are lined up in a desperate effort to blow variable ballast to sea. The 
process seems to take forever and the diving officer gives the count- 
down as he announces the increasing depth in ten foot increments. The 
submarine is well beyond test depth and into the Jesus Factor ’ before 
the lineup is completed and blowing commences. It is unfortunately 
too little and too late. 

It must be admitted that such an occurance is highly unlikely, but 
statistics over the past eleven years indicate that disasterous chains of 
events are not impossible. This scenario would have ended differently 
had the submarine not been out of trim initially. The purpose of this 
thesis is to derive and test an algorithm for a trim filter, that is, an 
electronic filter capable of trim analysis. 
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2 • Trim Analysis 

In the way of background, trim analysis is one of those facets of 
submarining that has been affected little by recent technological 
advances. It is the skill of estimating the required distribution of variable 
ballast to get the submarine in an "in trim" condition. Such a condition 
has been achieved when the submarine is capable of maintaining ordered 
depth with an ordered angle of pitch and with minimal deflections of 
the control surfaces. 

The ordered angle of pitch is most commonly referred to as 
'’ordered bubble and sometimes ordered trim angle The term bubble 
derives from the inclinometers used on older submarines. The instru- 
ments were inverted ” U" shaped glass tubes nearly completely filled 
with a colored fluid. A small air bubble always found its way to the 
highest point in the tube. An incremented scale behind the tube indicated 
the angle at which the instrument was inclined. Since they were aligned 
in afore and aft direction, the measured angle (or bubble if you will) 
coincided with the boat's angle of pitch. 

Sometimes, evolutions within the submarine dictate the ordered 
bubble. For example, while servicing torpedoes, it is frequently nec- 
essary to unlash torpedoes in their skids or raise the stop bolts which 
restrain them in torpedo tubes so that they may be loaded or unloaded. 
During the time that the weapons are free to travel longitudinally, it is 
wise to trim for a zero bubble so that the torpedomen need not wrestle 
unnecessarily with the forces of gravity. However, the prime candidate 
for ordered bubble is the so called 'neutral ' bubble. As the submarine 
changes speed, hydrodynamic forces and moments change in proportion 
to the velocity squared. When the submarine is at the neutral bubble, 
these changes are in equilibrium, and it is neither necessary to change 
the wieght of the submarine nor to shift the center of gravity. 

Necessary changes of variable ballast are accomplished with the 
trim system. Basically, it consists of tanks located throughout the 
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boat. The forward trim tank is in the vicinity of the bow; after 
trim tank, near the stern; and the auxiliary tanks near midships, 
number one to starboard and number tw’o to port. All tanks are con- 
nected to a trim manifold for which there are also water lines to and 
from a trim pump and the sea. Through the correct combinations of 
opened and closed valves at the manifold, it is possible to control the 
transfer of variable ballast. 

As was implied in the scenario, it is the duty of the diving officer 
to keep the submarine on ordered depth and, towards that end, to keep 
the submarine in trim. He observes the actions of the planesmen in 
their effort to maintain ordered depth and ordered bubble. Due to 
external disturbances and natural tendencies of the planesmen and the 
submarine, the planes are seldom stationary, nor is the bubble . The 
diving officer therefore mentally estimates the averages of those angles. 
Based upon those average angles, he then makes an estimate of the 
state of his trim, which is more qualitative than quantitative. If he 
deems it necessary, he issues commands to the trim manifold operator 
to correct errors in the submarine’s weight (W) or LCG(x ). Table 
I illustrates possible states of trim. "Stick'’ diagrams, such as that 



1. 


State 
In T rim 


W = Wo , Xq = X Go 


2. 


Heavy Overall (HOA) 


W>Wo 


3. 


Light Overall (LOA) 


W< Wo 


4. 


Heavy Forward (HF) 


HO A, XQ > XQo 


5. 


Heavy Aft (HA) 


HOA, XQ^ XQo 


6. 


Light Forward (LF) 


LOA, xq<xgo 


7. 


Light Aft (LA) 


LOA, xq>xqo 


8. 


Heavy Forward, Light Aft 


W = Wq, Xq> XQo 


9. 


Light Forward, Heavy Aft 


W = Wo. xg<xgo 


10. 


Alright Fore and Aft 


xq = ^Go’ ^OA or LOA 



Table I. States of Trim 



shown in Figure 2, are useful in representing the estimate of averaged 
angles. For this particular illustration, it is assumed that 
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Figure 2. ’Stick” Diagram 

the pitch angle is near neutral so that it contributes nothing to the state 
of equlibrium. The fairwater planes impart an upward force and a 
positive pitching moment, while the stern planes impart a downward 
force and a positive pitching moment. The net effect is a force near 
zero in the vertical direction and a positive pitching moment. The 
submarine is therefore heavy forward, light aft. The correct command 
would be Pump from forward trim to after trim. " The resulting shift 
aft of the center of gravity would provide the necessary hydrostatic 
pitching moment, so the planes should return to zero deflections. 

During the pumping operation, the diving officer would monitor the planes 
to ensure his analysis had been correct and to issue the command, 
’’secure pumping when the submarine was in trim. 



3 . Factors Affecting a Submarine’s Trim 

Once the diving officer has achieved the state of bliss known as 
being in trim, he will not be able to relax for the remainder of his 
watch. For unfortunately, the state of trim of a submarine is always 
in a state of change. On the brighter side, the rate of change is usually 
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slow. 

First, there are changes within the submarine. Provisions are 
consumed, water is distilled and consumed, and sanitary tanks 
are filled and evacuated. Bilges fill and are pumped. The crew is in 
a perpetual state of motion. Many an officer student has been victim- 
ized by a ’’trimming party ' composed of his classmates. As the 
student diving officer concentrates under the watchful eye of his in- 
structor, members of the trim party file by inconcpicuously from one 
end of the boat to the other. Just as orders are given to pump from 
forward trim to after trim, the trim party commences its migration 
from the bow to the stern. It is a wise diving officer who monitors 
planes, angle, and passageway. 

Environmental factors affect the submarine’s trim also. One rel- 
evant variable is the sea water density. As the submarine passes from 
one density to the next, the hydrostatic buoyant force changes and comp- 
ensation must be made. Near surface effects constitute another problem 
area. The boat is subjected to an abundance of external disturbances. 
There is an apparent suction between the boat and the surface for which 
compensation must be made. Deviations from mean angles become more 
severe for the planes and hull. Plane deflection indicators do not reveal 
the true angles of attack of the planes as nearby surface waves affect 
water particle velocities about the submarine. Under such circumstances, 
mental averaging becomes a. ore aifficult and trim analysis more clouded 
As one of Antony's oarsmen lamented to a comrade at the Battle of 
Actium, 'Theie must be a better way! ' 



4 . A Proposal 

The present technique of trim analysis could at best be described 
as adequate. The advent of nuclear propulsion has highlighted two of 
its major shortcomings. First, as speed increases, hydrostatic im- 
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balances become difficult, if not impossible, to perceive. Secondly, 
despite the increase in demand on their analytical abilities, diving 
officers find less time to learn and practice the art of trim analysis 
due the enormous demands imposed by nuclear propulsion. Never 
^ has it been so difficult or time consuming to master the art of submar- 
ining in all of its aspects. 

In the way of improvement, one suggestion has been to design a 
device which automatically computes averages for the plane deflections 
and angle of pitch. While this is credible in its simplicity, it falls 
short of the mark while the submarine is going through transients in 
depth. It was indeed an exceptional diving officer who could estimate 
variations in the trim as his boat was changing depth. 

The solution offered by this thesis is the trim filter. It is a device 
which estimates the response of the submarine to bounded disturbances 
and measured plane deflections. If the actual response differs from, the 
estimate, the filter revises its estimate of the state of trim. The result 
is a continuous process of trim analysis. As it turns out, a trim filter 
is not as complex as one miight expect 



■■■ 
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Chapter II 



Procedure 
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1 . General 

Optimal estimation techniques were investigated to find a method 
which was capable of the dynamic prediction and estimation mentioned 
in the previous chapter. The Kalman filter seemed to be an ideal 
solution. Basically, the procedure used was 

1. derivation of a mathematical model, 

2. controller design, 

3. filter design, and 

4. computer simulation. 

Most of the details on the above steps appear elsewhere in the 
thesis. However, a brief description follows. 



2. State Vector Model 

Kalman filtering requires that the system be described by a lineari- 
zed model in state vector notation. As described in reference (5), the 
general form for such a model is 

d/dt x(t) = A(t) x(t) +^(t) u(t) (II-l) 



where 

x(t) a state vector of n variables which completely describe 
the state of the system at any instant in time, 

u{t) = a control vector of r variables, 

A(t) = an n X n system coefficient matrix, and 

B(t) = an n X r control coefficient matrix. 

It can be seen that the rows of the state vector model are merely 
n first order differential equations. The output of the system is 
described as 
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y^{t) = Cft) x(t) 



(II-2) 



where 

y^(t) = an output vector of m variables, and 

C{t) = an m X n output coefficient matrix. 

The most obvious candidates for variables in the state vector 
are depth (z^), descent rate pitch (©), pitch rate (0), and forward 

velocity (u). Usually, terms related to the mass and center of gravity 
of such a system are included in_A(t). However, the nature of the trim 
problem is that these values are unknown. Rather than use classical 
parameter estimation techniques to compute the values of the coefficients, 
it was decided to include the submarine weight (W) and longitudinal 
center of gravity (x ) in the state vector. ( This serves two purposes. 

First and foremost, Kalman filtering facilitates the estimation of comp- 
onents of the state vector for which no direct measurements are available. 
Hence, a filter can be used to estimate the unmeasurable variables W and 
X . Secondly, the solutions and simulations are for constant propeller 
shaft rpm. Since ordered velocity is constant and variations in the trim 
are accounted for in the state vector, it can be assumed that the coefficient 
matrices are constant. Hence, equation (II-l) becomes 

d/dt x{t) - A x(t) (11-3) 

and equation (II-2) becomes 

X(t) - C x(t) (II-4) 

Finally, control surface deflections are included in the state 
vector. This is to facilitate the controller design since actually the 
rates of plane deflections are the control variables in depth keeping. 
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Appendix A describes the details of the derivation and linearization 
of the equations of motion. This is done about an equilibrium state 
(Appendix B), so that the components of the state vector are the devi~ 
ations from equilibrium. The complete state vector is therefore 




Notice that equation (II- 3) implies that the coefficients of x(t) on 
the left hand side is the identity matrix, However, the form cf the 

equations after the linearization is 

d/dt (Vx) = Wx + _Zu +_^ F (II-6) 

where, using the nomenclature of Appendix A, 

(II-7) 



F + F 

Z s Z ext 

F + F 

Ms M ext 

F + F 

Xs X ext 



U = 




(II-8) 



and the coefficient matrices are 
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( 11 - 12 ) 



Although this format appears cumbersome, it is a must for the 
steps which follow. The second, fourth and seventh rows of the matrix 
equations are simply the linearized heave, pitch, and surge equations 
of Appendix A. In a more straight forward notation, the remaining 
equations are 



d/dt z = z 

oe oe 

d/ dt © = 0e 

e 

d/dt 

d/dt - ic, 

d/dt x_ = O 
Ge 

d/dt W = O 
e 



( 11 - 13 ) 

ai-14) 

( 11 - 15 ) 

(11-16) 

( 11 - 17 ) 



( 11 - 18 ) 
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Multiplying both sides of equation (II- 6) by the inverse of V 
makes the equation resemble the required form. 

d/dt ^ (11-19) 



where 



A = V"^ W 



B = V‘^ Z 



( 11 - 20 ) 

( 11 - 21 ) 



and 



B' = V ZZ 



dl-22) 



The last term of equation (11-19) is merely a means of introducing 
steady state forces and external disturbances during the actual simu- 
lations . 



3. Depth Controller Design 

In order to keep the simulated submarine on ordered depth in the 
presence of disturbances and to effect depth changes, it was necessary 
to design a depth controller. Modern optimal control techniques were 
used "rather than classical techniques for two reasons. First, having 
derived a state vector model of the submarine, the equations were al- 
ready in the proper format for finding optimal feedback gains. Second, 
as is demonstrated in the appendices, the optimal control problem and 
the Kalman filter problem are solved by identical techniques. Hence 
the Author was able to capitalize on his experience with controller 
design when designing the filter. Also, the parallels between the two 
problems were instrumental in grasping the significance of the technique 
in its application to the filter problem , with which the Author was here- 
tofore unfamiliar. 
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Figure 3. Optimal Controller Structure 



Figure 3 illustrates the assumed structure for the controller 

design. The problem was simplified by not feeding back Au, x , and 

Ge 

W^. The state vector for the controller design was therefore 




and the model for determining controller gains was 



(11-23) 



d/dtx =A X +B u 
— c — c — c — c — 



(11-24) 



where, in terms of the elements of the original matrix, equation 
( 11-20 ), 
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and, as it turns out. 
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(11-25) 



(11-26) 



The details of the manner in which the feedback gains, K , 

— c 

were computed are discussed in Appendices C and E. Stated briefly, 
a quadratic performance index was selected and the gains emerged 
from the solution of the matrix Riccati equation. Potter’s method was 
found to be the most effective method of solution, regardless of the 
weighting matrices in the performance index. 

It is re-emphasized that the controller design is not the object of 
this thesis. It was merely a necessary step in the computer simulation 
of the submarine dynamics. 



4, Filter Design 

Figure 4 illustrates the basic relationship between the Kalman filter 
and the reduced physical system. Detailed descriptions of filters 
appear in references (5), (7), and (8). The general aspects are discussed 
below . 
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Figure 4. Kalman Filter Structure 

The purpose of the Kalman filter is to make a maximum likelihood 
estimate of the state of a system where variables are inaccessable and/or 
external and measurement noises are present. The theory is based 
upon the assumption that the disturbances are in the form of white noise 
with known statistical properties. This is rather a strict assumption. 

Its impact on the actual filter depends upon how wide the system' s 
bandwidth is compared with the disturbances' . The system equation 
is 



d/dt X£ = — f f ^ 



(11-27) 
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where ^ is a deterministic input and w. a random disturbance. To 
simplify the filter design, the filter state vector used was 
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for which (in terms of equation 11-20) 
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Two input vectors were used. The first was 




A U 



(11-29) 



(11-30) 



For which(in terms of equations 11-20 and 22) 
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( 11 - 31 ) 



The second input vector used was 
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( 11 - 32 ) 



( 11 - 33 ) 



Two sets of variables were assumed to be measurable. The first 
set is based on the assumption that the submarine is equipped with a 
modern inertial navigation system capable of measuring , ©^, 

and 0^. The real C^. matrix would have gains suitable for converting 
signal voltages from various instruments to signals corresponding to the 
actual v^alues of the measured variables. Without jeopardizing the validity 
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of the results, it was assumed that signals had already been converted 
so that 




1 0 
0 1 
0 0 
0 0 



0 0 
0 0 
1 0 
0 1 



0 0 
0 0 
0 0 
0 0 



(11-34) 



The purpose of the second set of variables used was to ascertain the 
validity of the application of the filter to more conventionally equipped 
submarines. In such cases there is no instrument for measuring 
directly, so that 
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(11-35) 



These matrices are used to describe the measurement signal, 
which is 



^ + V (11-36) 

According to theory, if the n X nm composite matrix 
[cT j i aT2cT ; . . . i aTO'-I’c't] (11-37) 

is of rank n, then the system is observable, ie. , a filter can be desiged. 
After the system was properly scaled, the above mentioned test was 
performed successfully for the first input vector (t^^. The scaling pro- 
vided for all angles to be measured in degrees, and in feet, 

in degrees / second, in feet /second, and W in megapounds. 
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The filter equation is 
d/dt ^ 

where 

A A 

Zf ~ ^ ^ 



(11-38) 



(11-39) 



The ideal means of calculating the feedback gains matrix, , are 
described in Appendices D and E. These techniques were first applied ■ 
to a system with input u^^ (equation 11-30). In practice, no solution was 
possible by the integration technique (time steps as small as 0,01 seconds 
were attampted), and Potter's technique ga\"e only a partial solution, the 
first four rows of the gain matrix. Hence, all that would have been 
possible was estimating the measureable variables. The object of this 
thesis being to estimate x^^ and W^, it was essential to find gains for the 
fifth and sixth rows of . This was done by using the positive results 
from Potter's method, supplemented by educated guesses. The tech- 
nique used is described in Appendix D. It was basically a pole placement 
technique. The final result was a filter that was two thirds optimal 
and one third "tweaked. " 

Subsequently, it was discovered that the problem was amenable to 
Potter's method if the second input vector, ^2 (equation 11-32), was used. 
The inclusion of the artifical noises, A x^^ and AW^, provided a 
mathematical means for the variation of the related parameters. Hence, 
gains could be calculated for compensation using the "optimal" technique. 

5. Simulation 

All simulations were performed on a IBM 360 computer. After the 
matrices had been set up, the differential equations were solved by a fourth 
order Runge-Kutta method, Hamming's modified predictor - corrector 
method was attempted first, but variables whose differentials were 
zero tended to "drift" rather than remain constant as they should have. 

A time step of 0, 1 seconds was used to make variables changed 
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Figure 5. Structure of Physical System Model, 
Constrained Controller, and Filter for Simulation 
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between time steps, such as controller or filter feedback and distur- 
bances, behave in a manner closely resembling continuous functions. 

The linearized equations of motion were used to simulate the 
submarine dynamics, and the density of the sea water and the acceler- 
ation due to gravity were assumed to remain constant. Figure 5 de- 
picts a block diagram of the simulation program which appears in Appendix 
F. The gains matrices, K and K , were calculated as described in 
the two previous sections. Both rate and position constraints were im- 
posed on the planes. 

Preliminary simulations were for the purpose of testing controller 
gains for given depth changes. Filter gains were tested for step inputs 
without and with Gaussian noise. Sensitivity to a ramp input ( flooding 
casualty) was tested and a least squares technique for smoothing was 
tested. All estimates were made while the submarine was periodically 
changing depth. 

The coefficients used appear in Table II. They were derived by 
manipulating data from a motley collection of books, reports, and articles 
on fluid drag, potential theory, bodies of revolution, airfoils, sub- 
mersibles, airships and naval propulsion. Although they represent no 
real submarine, the results obtained with them seem realistic, based 
upon the experience of the Author. It is therefore assumed that they are 
satisfactory for the sake of a numerical example and that the procedures 
described herein could be successfully applied to an actual set of sub- 
marine coefficients. The means of converting from non-dimensional 
form can be found in references (2) or (3)orinthe computer program for 
simulations in Appendix F. 
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Table II. Coefficients & Parameters for Simulations 
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I. "in Trim" Condition 



Figures 6 and 7 depict the solutions of equations (B-14) and 

(B-16) for W and x„ as a function of command speed (u ) and 
o Go ^ c 

ordered bubble It can be seen in figure 6 that a horizontal line 

drawn through \V-B = 0 would lie between = -1° and 2° , 

hence 



^ ^neutral ^ ^ 



(III-l) 



Looking at figure 7, it is seen that all curves are parabolic and 
that those in the vicinity of the bound expressed in (III-l) intersect 
at about 7. 15 knots. It is safe to assume that the curve for the neutral 
bubble would be a horizontal line passing through the intersection at 
7.15 knots. Therefore 

x„ . , = 0.025 ft (III-2) 

G neutral 



At zero speed where the only forces are hydrostatic, it can be seen 
that for each degree of trim, there corresponds a shift in x^ of about 
, 0185 feet. This fact can be used to linearly interpolate between -1° 
and -2° : 



^neutral ” ^ 



^Gneutral - ^G1 



G2 



G1 



(III-3) 



or 



0 



neutral 



-1 



0.025 - 0,0195 28° (III^ ) 



0. 039 - 0. 0195 



For the simulations, it was decided to use the nearest integer to the 
neutral bubble for ordered bubble, hence 

© = -1 

c 



for simulations. 



-B (kilopounds) 
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. Weight Minus Buoyancy vs. Command Velocity (u^) 



Figure 6 
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Figure 7. LCG vs. Command Velocity (u^) 
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Figure 7 is useful to demonstrate another point. Below 7, 15 
knots, trimming to pitch the bow down requires shifting the center 
of gravity forward as one might expect. However, above that speed 
the opposite is true. This is due to the domination of hydrodynamic 
moment over the hydrostatic moment. Although the transition is 
gradual, it can be said that, in general, hydrostatics dominate below 
7.15 knots and that hydrodynamics govern above 7. 15 knots. 

Since trim analysis is most difficult when hydrodynamics dominate, 
it was decided to test the filter well above the "transition" velocity. 
Therefore a command speed of 12 knots was used for all the simulations. 

2. Controller Tests 

Two sets of controller gains were calculated. The weighting 
matrices for the first were; 



100 0 0 0 0 0 

0 0 0 0 0 0 

0 0 4 0 0 0 



Scl 



0 0 0 0 0 0 



(III-5) 



0 0 0 0 0 0 



0 0 0 0 0 0 



and 



1 



0 




(III-6) 



0 



1 



and the resulting gain matrix was 



8. 721 43.14 -3.579 -34.53 -.6673 -.008933 




4.894 17.82 1,497 1.913 -.008932 -.4408 



(III-7) 
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Figure 8 depicts the response of the "controlled" submarine 
to an order to decrease depth by 50 feet. When last seen, the sub- 
marine was passing 4000 feet with a pitch angle of -116° ! Crews 
have jokingly given diving officers quarters for much less exciting 
rides. 

Going back to the weighting matrices, the logic in was that 
maintaining depth was far more important than ordered bubble which 
was the only other variable worth weighing. Most diving officers would 
have pitched up about 5° for such a depth change, but the "optimal" 
controller which is actually a linear regulator does not account for 
the constraints on the planes' deflections. Even though the error in 
depth far exceeds the bubble error, the controller "thinks" it can 
change depth in an optimal manner while maintaining a pitch angle 
close to the ordered bubble by using plane deflections exceeding 180° if 
the need exists. This highlights two short -comings: the failure to 
account for constraints and the failure of linear theory to account for the 
"stalling" of control surfaces at large angles of attack. 

By the time became large, became much larger and was 
still the predominant error. With luck, this controller might have 
accomplished a depth change of 8 feet. 

An attempt was made to calculate a set of gains for which there was 
no weighing of pitch angle error, however, the Riccati equation was un- 
solvable for such a case which the techniques at the disposal of the 
Author. Therefore a compromise was made for the second set of gains, 
©g was weighted more heavily as was u^ . The matrices were: 




9 0 
0 0 
0 0 
0 0 
0 0 
0 0 



0 0 
0 0 
400 0 
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0 0 
0 0 



0 0 
0 0 
0 0 
0 0 
0 0 
0 0 



(III-8) 
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Figure 8, JK^j^ Depth Controller Simulation 
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and 



K 



c2 



.2999 2.996 -.7980 -6,049 -.2036 .082163 

.0068262 1.103 2.147 8.762 .082163 -.3585 



(III-IO) 



Comparing K ^2 ''•''ith , one can see the decrease in the total 

control effort and the increase in the relative importance of © over 



Figure (9) shows the typical record of depth change orders 
and submarine response for the remainder of the simulations, 

3. Filter Gains 

Four filter feedback gains matrices (K^ ) were calculated. The 
matrices and relevant data appear in tables III thru VI, Unless indicated 
otherwise, off-diagonal elements of square matrices are zero. 

Also note that S, , S , andAu are treated as external disturbances, 

D S 

The noise assigned to these variables can be used to imply the un- 
certainty which exists in their physical impact on the submarine dy- 
namics, For example, it is the angle of attack of the control surfaces, 
rather than their deflections relative to the submarine, which determines 
the forces and moments they impart on the submarine. The greatest 
degree of uncertainty for these angles exists near the surface. 
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Table III and Related Data 



Speed: 12 knots; Ordered Bubble: -1° 
Input Vector: (equation 11-30) 

Measurement Matrix: (equation 11-34) 

Technique: Potter /pole placement 
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. 1772 degrees 


^/100„ 


FZext 


0 


1, 772. 45 pounds 


TTX 10^ 


FMext 


0 


17, 724. 5 foot pounds 


TTX 10“ 


Au 


0 


. 1063 feet/second 


. 0113 


^oe 


0 


. 8862 feet 


. 7854 


^oe 


0 


.8862 feet/second 


. 7854 


©e 


0 


. 1772 degrees 


'it/lOO 


©e 


0 


.1772 degrees/ second 


1T/100 




4T /lOO 

4T/100 f, 
itx 10 

ttX 10 



. 0113 




. 7854 



. 7854 

TT/100 

TT/100 




0, 052295000 
0, 001507200 
003330200 
0, 000090246 
0. 000450000 
0,000030000 



0. 0015072000 
0. 0001506800 
-.0004266000 
-. 0000024576 
0. 0000200000 
0. 0003200000 



-. 0832560 
0106650 
0. 0485490 
0. 0013203 
-0020000 
0. 0000000 



0. 002256200 
000061439 
0. 001320300 
0. 000225530 
-004500000 
0. 000000000 



Eigenvalues 



Real Part 
-. 1287 
03371 
-. 03371 
-. 04259 
04259 
-. 01234 



Imaginary Part 

0 . 0 

. 06354 
- 06354 
. 0353 
-.0353 
0 . 0 
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Table IV K ^2 ?? elated Data 

O 

Speed: 12 knots; Ordered Bubble: - 1 
Input Vector: (equation 11-30) 

Measurement Matrix: (equation II- 34-) 

Technique: Potter /pole placement 



Variable E(w or v) 



^b 0 

Ss 0 

FZext 0 

FMext 0 

Au 0 



Zoe U 

zoe 0 

©e 0 

0 



2 2 

Standard Deviation E(w or v ) 



1 degree 1 

1 degree 1 

1 pound 1 

1 foot pound 1 

1 foot/second 1 

. 03162 feet . 001 

.03162 feet/second .001 

. 03162 degrees . 001 

.03162 degrees/ second .001 




. 001 

. 001 





.001 










• 


001 






0. 470800 


0. 118200 


-. 018969 


0. 022660 


0. 118200 


0. 075907 


-. 043117 


0. 012530 


018969 


-. 043117 


0. 501400 


0. 136100 


0. 022660 


0. 012530 


0. 136100 


0.099234 


0. 003000 


0. 002000 


-.032000 


-.450000 


0. 004000 


0. 016000 


0. 000000 


0. 000000 


Filter 


Eigenvalues 






Real Part 




Imaginary 


Part 


-. 3287 






. 3760 




-. 3287 






-. 3760 




1813 






. 2268 




-. 1813 






-. 2268 




-. 2430 






0.0 




-.07681 






0. 0 





Table V 



and Related Data 



Speed: 12 knots; Ordered Bubble: -1 
Input Vector; ^2 (equation 11-32) 
Measurement Matrix: (equation II-24) 

Technique: Potter 



Variable E(\v 


or v) 


2 

Standard Deviation E(w 


2 

or V 


^b 


0 


2 degrees 


4 


Ss 


0 


1 degree 


!o“ 


FZext 


0 


105 pounds 


FMext 


0 


10 foot pounds 


10l4 


Au 


0 


2 feet/second 


4 




0 


0. 1 feet 


. 01 


Avf/ 


0 


0.05 megapounds 


. 0025 


?oe 

^06 


0 


0. 5 feet 


. 25 


0 


0. 2 feet/second 


. 04 




0 


0. 1 degrees 


. 01 


©e 


0 


0. 1 degrees/second 


. 01 



^£3 



lOlO 



10l4 



01 



. 0025 



p 

-£3 



-£3 



25 



. 04 



01 



0.38940000 
0. 12340000 
00018560 
0. 00091933 
0. 00481060 
0. 02275400 



.01 



0. 771500 
0. 801800 
020016 
-.021942 
0. 033007 
0. 242600 



-.0046404 
-.0800630 
0. 8350000 
0. 4512000 
-. 5456000 
0. 035506 



0. 022983 
-.087767 
0. 451200 
0. 677100 
-.835100 
0. 084360 



Table VI and Related Data 



Speed: 12 knots; Ordered Bubble: -1 
Input Vector: (equation 11-32) 

Measurement Matrix: C ^2 (equation 11-35) 
Technique: Potter 

or v) 



Variable E(w 

"^b 
S s 

FZext 
FMext 
Au 
Ax, 



AW 



Ge 



0 

0 

0 

0 

0 

0 

0 



2 2 

Standard Deviation E(w or v ) 



2 degrees 

1 degree 
10^ pounds 

lO'^ foot pounds 

2 feet/ second 
0. 1 feet 

0, 05 megapounds 



10^4 

4 

. 01 
0025 



Zoe 



0 

0 

0 



0. 5 feet . 25 

0. 1 degrees . 01 

0. 1 degrees/ second . 01 



Qf4 



10 



10 



10 



14 



01 



. 0025 



Pf4 



25 



. 01 



.01 



Kf4 



0. 9680000 


0.0 


-.048367 


0. 082643 


0. 4687000 


0. 0 


132600 


-. 064191 


-.0019347 


0.0 


0. 836400 


0. 451900 


0. 0033057 


0. 0 


0. 451900 


0. 678400 


0. 0113010 


0. 0 


-. 546800 


-. 835400 


0.0998360 


0. 0 


0. 019525 


0. 020985 



The actual dimension of are 6X3. The above format is required 
for the computer simulation program (AppendLx F). The column of 
zeroes implies that z is not measured and that z is not fed back 



oe 



oe 



to the filter. 



4. Filter Simulations 

Filter simulation results are grouped by gains matrices. Re- 
sponses to step and ramp (flooding) inputs were simulated. The step 
inputs in were plus or minus 11,000 pounds. Table \TI illustrates 
the comparison of this error to various types of displacements for 
the test submarine. Other than noise-free 



Classification of Displacement 


Displacement (tons) 


Wg/ Dis- 






placement 


Hydrodynamic (incl. free flooding) 


7392 


. 0746% 


Submerged 


7170 


. 0767% 


Surfaced 


6100 


. 0902% 



Table VII, Comparison of Step to Displacement 

simulations to determine the natural responses of the filters, two 
sets of noises were used to provide a common base for comparing 
responses. The statistical properties of these Gaussian disturbances 
appear in tables VIII and IX. 



External Measurement 



Variable 


E(w') 


Standard 


Variable 


E(v) 


Standard 






Deviation 






Deviation 


FZext 


0 


1, 000 lb 


^oe 


0 


. 5 ft 


FMext 


0 


10,000 ft-lb 


^oe 


0 


. 5 ft/sec 


FXext 


0 


500 lb 




0 


. 1 








^e 


0 


.1 /sec 








?b 


0 


. 1 








Ss 


0 


. 1 








A u 


0 


. 06 ft/sec 



Table VIII, Disturbance Statistics 
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External 






Vleasurement 




Variable 


E(w) 


Standard 


Variable E(v) 


Standard 






Deviation 






Deviation 


FZext 


0 


1 lb 


z 

oe 


0 


. 0316 ft 


FMext 


0 


1 ft -lb 


^oe 


0 


. 0316 ft/sec 


FXext 


0 


1 lb 


©e 


0 


. 0316 








©e 


0 


. 0316 /sec 










0 


1 








^s 


0 


1 








2^u 


0 


. 0113 ft/sec 



Table IX. Disturbance Statistics 

Figure 10 and 11 illustrate the natural responses of filters 
equipped with (Table III) and ^2 (Table IV) respectively. It 

can be seen that the response of is much slower than ^ 2 * The 
disturbances of Table VIII exceed those for which K ^2 was designed for 
the most part, but are mostly less than those for which was 
designed. Comparing the responses (figures 10, 12, and 13) it can be 
seen that the slower filter (^j^ ) is more stable. Note also, that even 
though the faster filter (K^^ is underdesigned, the means of the estimates 
converge toward the actual trim errors. This suggests some sort of 
smoothing might be used to salvage the output of the filter. The rel- 
atively large overshoots of for the slov/er filter ( (figure 10) 

can be attributed to lack of damping. This is also reflected in the 
eigenvalues (Table III) for , The imaginary parts of one pair are 
nearly twice as large as the real parts. 

The disturbances of Table IX were used for subsequent simulations. 

They are the maximum distrubances for which was designed, and this 

filter's response appears in figure 14. The coupling between x_ and 

W can also be seen. "Undershoots" in W lag overshoots of x„ by 
e e ° Ge 

about ten seconds. 

Figures 15 thru 18 were plotted for a filter with 



An attempt 
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to plot the response of to the same conditions (step input) 

revealed that the differences between the two filters could not be 
discerned with the scales used for the graphs. The response of these 
filters (figure 16) is much faster than the previous two filters. In 
fact, the new filters are faster than the. submarine in a sense. At 30 
and 60 seconds, depth change commands were given, causing the planes 
to deflect. Before the submarine could respond to the planes, the filter 
"assumed" the plane deflections were due to steps in trim error. Hence, 
the apparent disturbances of the noiseless simulations at those times. 
This might also explain the fluctuations for the filter with pi'ior 
to settling out (figure 11). 

When subjected to the disturbances of Table IX, the two new filters 
suffered from relatively severe fluctuations (figure 15) which were 
smoothed somewhat using a least -squares smoother (figure 16). Data 
from the previous thirty seconds was used for smoothing estimates. 

The noise was less than that for which the filters were designed. The 
smoothed estimate of x,^ lags the unsmoothed estimate by about six 
seconds while that for W lags the unsmoothed estimate by only one 
second. This might be attributed to the fact that the response of W 

A ^ 

is faster than that of x„ (see figure 16). 

A filter equipped with was subjected to a flooding casualty 
(figures 17 and 18) of 500 pounds /second, from 5 to 60 seconds, at 125 
feet aft of the body axes' reference. The simulation was begun at -30 
seconds with zero estimate errors to ensure the filter had "settled out" 
prior to the casualty. The smoothed estimate of lags the beginning 

of the casualty by about 12 seconds and gets within 5 seconds, while 
that of W^ lags by 5 seconds at first and gets within 3 seconds, ex- 
cluding the pertubation at 35 seconds due to the plane deflections. 
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(kilopounds) 
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•'Depth 




Figure 10. K., , Step Input, No Noise and 

Noise (Table VIII) 



(feet) 



(Kuopounas; 
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=:=Depth 

Change commands 




Figure 11. K ^2 » Noise 



(feet) 



*Depth Change 




Figure 12. ^2' Step Input, Noise (Table \ HI) 
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-Depth Change 






I 



(kllopounds) 
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*Depth Change 
Command 




(feet) 



(kilopounds) 



=i=Depth Change 
Commands 










O to 20 30 4-0 50 &0 70 80 90 lOO 

t ime(seconds) 

Figure 15, and , Step Input, Noise (Table IX) 



o 



r " 

X 






(feet) 



(kilopounds) 
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*Depth Change 
Commands 



-Hr Xr 




time(seconds) 

Figure 16. and , Step Input, No Noise 
and Smoothed Noise (Table IX) 



(feet) 



(feet) 
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-!=Depth Change 
Commands 




Figure 17. , Flooding Casualty Noise 

and Smoothed Noise (Table IX) 



OU.V\ 



*Depth Change 
C ommands 




Figure 18, , Flooding Casualty W^, Noise 

and Smoothed Noise (Table IX) 



Chapter IV 



Conclusions and Recommendations 
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1. Conclusions 

It has been demonstrated that trim analysis lends itself to optimal 
estimation techniques which manipulate measurements from instru- 
ments installed on most U. S. Navy submarines. Sufficient measure- 
ments are those of depth, pitch, and pitch rate. Installation of a rate 
gyro should be a simple task if no instrument, capable of measuring 
pitch rate, is already installed on a given submarine. Although not 
required, measurements of rate of descent are useful if available. A 
use by-product of a Wiener filter designed for this task is its ability 
to estimate the severity of a flooding casualty. This information could 
be of benefit to an officer of the deck by alerting him of the casualty 
and by providing him with information necessary for determining the 
extent of recovery action required. These estimates can be made while 
conducting maneuvers in the vertical plane. 

A potential problem area exists in the variation of the physical 
system from the linearized filter model. The simulations described 
herein are "first cut" in that the filter model coincides with the true 
system model. In reality, there are two sources of error here. First, 
the coefficients of motion might not be accurate. Secondly, as devia- 
tions from the equilibrium conditions become larger, the accuracy of 
the linearized equations of the filter is degraded. Motion in the hori- 
zontal plane is another deviation for which the filter is not equipped . 

It can be expected that modeling errors will be reflected by the filter 
as measurement and estimate errors. Filter instability is another 
possibility. Being specific about differences in coefficients, some of these 
variations might be predictable and lend themselves to updating by a 
computer. Concerning deviations from equilibrium conditions, the 
control surfaces are probably the worst offenders. However, they are 
treated as inputs by the filter. Hence, they need not be linear, and it 
would be satisfactory if forces and moments were stored in a computer 
as functions of plane deflection. Filter gains would remain the same. 

The moment of inertia can also change appreciably, depending on how 
water is distributed in the trim system. This phenomenon is some- 
times utilized by submariners to "tune" the resonant frequency of the 
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submarine in pitch near the surface. It might be advisable, therefore, 
to feedback water levels in the trim tanks to the filter. 

Another potential problem area is that of disturbances. The filter 
design is based upon the assumption that they are stationary, normally 
distributed random variables with known statistical properties. While 
it might well be possible to approach this situation for the measure- 
ment distrubances, it is quite another matter for external disturbances. 
Although no problem exists while the submarine is deep, near surface 
effects can be very dramatic. In fact, the submarine's response might 
be very "broad band" compared with typical sea spectrums. It is 
under such circumstances that the smoother becomes valuable. The 
best period over which the smoothing should be done might well be the 
subject of another thesis. However, if the trim error changes very 
slowly, which it usually does, the luxury of longer periods could be used 
to obtain smoother estimates. 

There are other sources of error which have been neglected such 
as hull compressibility effects and free surface effects of fluids in tanks 
or the bilges. It is felt that if not already capable, the filter can be 
suited to such problems by imaginative uses of the covariance matrices 
to reflect uncertainties. 

Concerning filter gains, susceptibility to noise varies inversely 
with filter response time. For a true Kalman filter, the gains are 
functions of time so that initial errors are heavily weighted to ensure 
rapid convergence followed by relatively smooth tracking. W’hile this 
is good for variables with large variations, it is not felt to be necessary 
for trim error variables. This might not be true if the filter is intended 
to detect and measure flooding casualties. Since these objectives are 
contradictory, it might be well to consider two separate filters rather than 
one which does neither job well. 

It is not expected that real time applications would be difficult, 
given the present state-of-the-art of computer technology. Despite 
the Author's usage of the relatively slow fourth order Runge-Kutta 
technique, complete problem set-ups and simulations took about half of 
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the real time simulated on the IBM 360 computer. 

2. R ecommendations 

Before the actual application of such a filter to a test platform, 
the following factors should be considered: 

The filter must be manipulated to be as sophisticated as is useful 
for the given hardware. This means considering such factors as 
making the filter discrete or continuous. Shall the gains be calculated 
on the submarine's computer, or precalculated and stored therein? 
Should the system matrices be precalculated or stored? Before making 
such decisions, an extensive sensitivity analysis should be conducted to 
ascertain the effect of previously described uncertainties on the filter's 
performance. Most challenging would be the study and simulation of 
near surface effects. 

Reference (7) is packed with practical considerations and, in the 
opinion of the Author, could be of immense value in any such under- 
taking. Contained therein are many examples of and inovative solutions 
to such problems. 
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Submarine Equations 
of Motion in 



the Vertical Plane 
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1. General 

The first st^^p in the solution of the control and estimation 
problems is the derivation and linearization of the equations of 
motion of the submarine. In order to restrain the magnitude of the 
problem, this derivation is limited to the three degrees of freedom 
in the vertical plane. These are pitch, heave, and surge. Although 
this constrains the applicability of the results somewhat, the constraint 
should not be too severe for two reasons. First, submarines spend the 
vast majority of their underway time traveling on a straight course 
between the proverbial points A and B. Secondly, for most course 
changes, it is expected that the coupling between the vertical and hori- 
zontal planes will not be so severe as to render the trim filter useless. 
In fact, most of the coupling is through second order terms which would 
be dropped in the linearization process herein. In short, it is not felt 
that the slight increase in accuracy during relatively rare manuevers 
justifies the added complexity brought upon by the added three degrees 
of freedom in the horizontal plane. 

The basic forms of the equations are: 

F - d/cit i m y_(3.) (A-i) 

and 




(A-1) simply says that the rate of change of momentum of a body 

relative to an inertial reference is equal to the resultant of the applied 

forces. Likewise, equation (A-2) implies that the rate of change of the 

angular momentum of a body is equal to the applied torque. Figure 1 

on page xx illustrates the axes systems. The x and z axes are 

o o 

fixed relative to the earth and are directed horizontally and vertically 
downward respectively, earth will be assumed to be an inertial 

reference. Such an assumption will cause negligible errors even for 



those relatively high speeds attained by submarines returning from 
extended deployments. 

The X and z axes are the body axes of the submarine. Their 
origin is the point assumed to be the body axes origin during the deri- 
vation of the coefficients of motion. However, due to the changes in 
the position of the center of gravity relative to the submarine for 
various trim conditions, the origin of the body axes and the C. G. will 
rarely cdncide. Furthermore, the x-z plane is also a plane of 
geometric symmetry of the submarine. This is also assumed to be 
a plane of mass symmetry. Hence, the y axis which is perpendicular 
to the X- z plane, is assumed to be a principal axis of inertia. 

2. Inertial Forces and Moments 

Using the techniques described in reference (1), the right hand 
sides of the basic equations can be broken down into the various com- 
ponents of the inertial reactions relative to the submarine's axes origin. 
(Wherever possible, the standardized nomenclature of reference (2) 
has been utilized. ) Starting with equation (A-1), 



Normally, rates of change of the mass and the center of gravity of 



omitted. In this case, they shall be retained for the sake of generality 
during trimming or flooding of the submarine. 

The velocity of the CG becomes 




(A-3) 



the vehicle are considered negligible for marine applications and are 




(A-4) 



where 



U 




(A-5) 
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(A-7) 



(A-6) 




(A-8) 



Hence, 



XGrCjk 



(A-9) 



After appropriate substitutions and grouping of terms, equation 
(A- 4 ) becomes 



It can be seen in equation (A-ll) that there are five components 
in the acceleration of the CG. The first is that due to the acceleration 
of the body axes relative to the inertial axes. The second is that 
due to the translational acceleration of the CG relative to the body axes. 
The third is due to the angular acceleration of the body axes and the 
fourth is the centripetal acceleration due to the body axis rotation. 



+ l + (w + w^,- x^c^)k 



A-10) 



Next, the acceleration of the CG must be described. 






(A-11) 
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The last contribution is that of the Coriolis acceleration. It is now 
necessary to reduce the components of U ^ further. 
Differentiating equation (A-5) with respect to time, 




U L W 



(A-12) 



The first two terms on the right hand side of equation (A~12) 
represent the translational acceleration of the body axes. The second 
two terms reflect the change in the orientation of the unit vectors due 
to the rotation of the body axes, ie. , 



dl/d-t =ny I = - 

and 7 

dk/dt=nxtl= 

Hence, equation (A-12) becomes 

U - Cu wcj) L ( w - U Cj) k 



(A- 13 ) 



(A- 14 ) 



(A-15) 



Also, 




U 






A. 

L 



+ 



W 



>-e\ 







So, 



axR,= 



A A 

L X<^Cj k 



(A-16) 

(A-17) 



(A-18) 




■M 
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Using equation (A-9) 

n Xnv Rg= - Xr-Cj^ I ~ k (A-19) 

Finally, 

A A. 

2. - 2 L ~ Z c| k (a-zo) 

Substituting equations (A-15, 16, 18, 19, and 20) into equation (A-11) 
and grouping terms yields. 







-*■2 

-2 



A, 

C| u^^i) k 



(A-21) 



Next equations (A-10 and 21) are substituted into equation (A-3), yielding 

' d /d-b (m L '*■ k (A- 22 ) 

"vhere the surge component of the rate of change in linear momentum is 







and the heave component is 

rri (w- ua 4- x^<^~ :Ze,cj^-2c| u^-ei) 



(A-24) 
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It is necessary to re^s^rite both sides of equation. (A-2) 
stage. Expressing torque about the body axis, 


at this 


T^=T-R,xF 

where 


(A-25) 


E.^£= x^Zj j 

On the right hand side of equation (A-2) is 


(A-26) 


H^=X.n 


(A-27) 


Differentiating equation (A-27) wdth respect to time, 

d/citH^= 

or 


(A-28) 


where 


(A-29) 


I&y = ly-m CxJ + -z.%) 

and 


(A-30) 


- m (z Xc^u^^l 


(A-31) 



Combining (A-25, 26, 29, 30, and 31 ) and rearranging terms 



yields 
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T-C(z^^X^- +C| ly 

A (A-32) 

- C|m(x^-^ Z^) ~ cjm Zz^w^^j)] j 

Also 

T = M^] (A-34) 

Several cancellations occur when equations (A-23 and 24 ) are 
substituted into equation (A- 32). Regrouping terms and equating the 
results to equation (A- 34) yields 



■^m[ Z(^(u + WC^+U^^,)- x^(w-uc|+ vv^.^^)] 

This completes the modification of the right hand sides of equations 
(A-1 and 2). Now the left hand sides must be more adequately described. 



3. Applied Forces and Moments 

U^RT^erence (3) gives a comprehensive breakdown of the applied 
forces and moments. The coefficients are partial derivatives of the 
hydrodynamic force or moment with respect to the subscripted variable. 
For example, 






I 



% 
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For the vertical plane, the surge component of the applied forces is: 



-^X. 0 +X^^WC| + 

B-(_ u ~(W~ B) 51 n 0 

^ (V? - 1) - (uy u^) [X,,,, st y 

(sur^e) 



(A-36) 



(^"Ol+SlDiS-turlD 



ances 



The heave component of the applied forces is: 



^ +Z^w + Z^,^,w |c|! +Z^,^jwlwl 
+ (W“B) COS0 + w Iwl (^"l) +(u/u) [Zcj 

h' w Iwl [z^^ -^z^^w] 

•(n-i)jH-(uVuy[z.-z,,^,^z,,y 

I stur loanees ( heave) 



(A-37) 
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For the vertical plane, the only component of the applied torque 
is the pitch component. The applied torque in pitch is: 



+ hl^l^|Wlwl + M^^w"--(x,jW-XgB)cos© 
-(z,j.W-ZaB)sin© + M^,^„,w|wl('a-l) 

+ (u/U) [M,, C| + + M^W 

C| •*- w] (V-Ol (A-38) 

+ (uV U KM*+ 

ances ( pliicVi) 

4. Change of Variables 

Generally, the motions of a submarine in the vertical plane are 
described in terms of ahead speed (u), pitch (©), and depth ( with 
the origin at the sea surface). These are illustrated in Figure 1 
on page xx . This is due in part to the measurements which are 
available. The preceeding equations of motion must be modified so 
as to use the available measurements directly. The required change 
of variables is accomplished using the following equations: 



i 

I 

t 



I 
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Zq~ w cos© ~ u sin 0 



(A-39) 



- w cos 



© - 



WC| s 



itn © 



u sm0 "■ u c| COS0 



(A-40) 



Furthermore, for the control and estimation aspects of this thesis 

it is useful to express the variables in terms of command depth 

(commonly referred to as ordered depth), z ; deviation from ordered 

oc 

depth, z ; command pitch (ordered bubble), © ; and deviation from 
oe c 

ordered bubble, 0 . 

e 



Hence, 
and 

© = e. - e. 

Since © and z are piecewise constant, 
c oc 



(A-41) 



(A-42) 



oe 



(A-43) 




(A-44) 



Cj= ©^ (A-45) 

, and 




(A-46) 
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The following additional transformations result: 



w z: [ COS (0 + Q)~\ u -tan 10-^9) 

oe c e c. e' 



(A-47) 



, and 




-^ ( W 0^ + U ) "tairi (0c + 0e) 



(A-48) 



5* Linearization 

The application of optimal control and estimation theory is well 
served if the equations of motion are linearized. This is accomplished 
using the techniques described in reference (4). Basically, a Taylor’s 
series expansion of all variables is made about an equilibrium condi- 
tion. After substituting these expansions into the equations of motion, 
terms of higher than first order are omitted. For small perturbations 
about the equilibrium condition, the actual equations are very closely 
approximated by the linearized equations. As the magnitude of the pertu- 
bations increases, the accuracy of the linearized equations is degraded. 

The equilibrium condition used will be for the submarine traveling 
horizontally at a steady speed and with a steady pitch angle. The 
object of this thesis is to determine the magnitude of pertubations 
of the submarine’s trim from the "in trim ’ condition, therefore, 
as part of the equilibrium condition it is necessary to include the 'in 
trim” state of weight and longitudinal center of gravity. More 
details on the equilibrium conditions appear in Appendix B. The 
linearized variables are as follows: 




Sin 0c. 



cos 



(A-49) 




I 



H 
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COS0 - COS 0^ “ sm (A- 50) 

Practically speaking, unless the submarine is grossly out of 
trim, the magnitude of the ordered bubble for depth keeping purposes 
is always less than seven degrees. Therefore, the second term in 
equation (A- 50) is of the same order of magnitude as a second order 
term and can be dropped, leaving 

cos 0 = cos 0^^ (A-51) 

Other linearized variables are 

•tan © = ©g ■*• -tan (A-52) 

u = •'• A u (A-53) 

u = A u (A-54) 

A (A-55) 

W = VI ^ * Wg (m= trig) (a-56) 

W = W (A-57) 

ly = + A <^-58) 

iy= a1^ (A-591 



(A-60) 
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(A-61) 



^ = A 6| 



(A-62) 



Linearized products of certain variables are: 



u i:an0 ~ 0^ u^i:an0^ A u 'tanQ. (a-63) 



Using equation (A-63) to linearize equation (A-47) yields 



w "■ zig^Ccos©^) ^ i:an0^ (a-64) 

Au "tan Q. 



which can be used in the linearization of equation (A- 48): 

w = ( cos ©^) ' 0 tan^0c) ©e 

(A-65) 

u "tan 

From equation (A-64) it can be seen that 

vv = u "tan 0. (A-66) 

o ^ ^ 

and 

AW = ( cos 9c) * + u^0g + A u iran 0^ (a-67) 





I 
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These variables are used to more clearly illustrate the following 
linearized variables; 



IwJ - j-tan 0J 


(A-68) 


w — ^ Wq aw 


(A-69) 


W 1 W 1 = Wq 1 1 "*■ 2. 1 Wq 1 AW 


(A-70) 


Also, 




0 

II 

cr 


(A-71) 


wcj - -tan ©e 


(A-72) 


+■ Z A u 


(A-73) 


^ A 


(A-74) 


= A 


(A-75) 


o< — -1- ^ cx 


(A-76) 


cos = cos ^ ^ ^c. 


(A-77) 



Once again, since © is expected to be less than seven degrees, the 

c 

second term of equation (A-77) is of the same order of magnitude 
as a second order term. Therefore it is dropped, leaving 

~ cos (A-78) 

For ahead motion only 







I 
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Since U ~ U / COS 



the linearized version of velocity becomes 

U = ( Uo ^ ^)/ (A- 79) 

Hence, 

u/U = 1/ cos ©c_ (A-80) 

For the axial propeller thrust equations 

^7 = / U (A-81) 

where 

^ 

for variable rpm simulations. 

Linearizing equation (A- 82) 

Substituting the components of equation (A-79) in equation (A- 83) yields 

H ~ cos /uo A COS ©^ / (A-84) 

- A U COS ©^ / uj 



Therefore, 




I 
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h-l) - (u^o ~ \ 

(cob /u,) A CosB.^/ut) AU (A-85) 



For the sake of the simulations herein, rpm is considered to remain 
constant. This does not affect the trim filter in that it does not attempt 
to estimate changes in surge. The filter merely uses the measure- 
ments of Au . This policy simplifies equation (A-85): 



C^-l) - AU (A-86) 

where 

/Uo) " ^ 

and 

^>12 "" ~^co COS0e/ Uj (A-88) 



This completes the bulk of the linearization of the variables with 

the exceptions of w u w . and . For the linearized equations. 

rel rel rel G 

it shall be assumed that changes in the height of the CG are extremely 
small for all practical cases. Hence, the effect on the dynamics of 
the submarine as predicted by the linearized equations v/ould be negli- 
gible. From the assumption that 



O 

it follows also that 



W 



K-el 



- o 



(A-89) 



^re\ - 



(A90) 




I 
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for the linearized equations. 

In the longitudinal direction, there exists the possibilities 

of much larger moment arms for flooding and trimming water. Hence 

A is certainly a variable to reckon with. The rate of change of 

A X (u ) can be expected to be small for typical rates of flooding 
G re 1 ® 

or trimming. Due to coupling with other variables, u does not 

r el 

emerge from the linearization process as a first order term, so one 

does not have to make a practical decision as to whether or not it 

should be retained in the linearized equations. This is not the case 

for u D so the assumption, is made, in light of the fact that u is so 
rel ^ rel 

small, that 

(A- 91) 

for the linearized equations. 

Substituting the linearized variables into equations (A-23) and (A-36) 
and arranging terms yields the linearized surge equation: 



C'nn^- X^) u + (uVg) W 

(X^^/cos 0,) + X^ee 

AU- sm0,W^+ 

where 

X^^ - (W^- B) COS 

Xav. 2 Cvi, 

-tan 



(A- 93) 
(A- 94) 
(A-95) 




I 
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'^o rn = ) 

^Au 2. U„(Ai + 

(A-97) 

+ X^^-tan©^ 

Fxs" C,,,X^„,) + C^uJ 

'^'-'Xu.CAi+y^^)+B^u^^-(v4-B) 



and and defined by equations (A-87) and (A-88). 

Equations (A-24) and fA-37) lead to the linearized heave equation: 



©e + tan ©, u 

(w„/g) W = CZ^^ / cos©J -*- 

©e+ COS^e^Z^^ <A-99) 

Z^u AM + cosG^Wg + 

where 

/cos0^ 

7 • “ rn — z^ • 

*■ AW ‘ ' ' o v> 



(A-101) 




I 



(A- U)Z) 



Z- 



A© e 



~ X 



CrO 



m 



o 




z 



AW ^ \^~W|vx/l * 2- 

-coseZZ^+ C„Z.„, 



(A-103) 



Wq C^-wh.vl'i 



c.« (z, 

+ Z^v^/'tan ©c 



(A-104) 



Z 



AS.S 



c <- 

a 



+ c„,z 






COS 



^ 0, 



(A-105! 



= ( \a/ — R 0* z" 1 w 1 Z cc^ CO s O 

■ ZS '■ ■ O >—' Iwl ‘ ‘-Z <- 



4- 



VA. 



Iwl 



wivv/R ^'T.l 



(A-106) 



-t-w "7 '^(Z C-^- 7 „ j cos 0^ 

o v^y VT WVO / 



z • -- 

— A 0 e 


-4- C Z^ ) 

<^1 — ^ — C5jV7 


cos ©c 


(A-107) 


-4- 


Uo[n%“ Z^c/ C ^ 


+ -baZ ©c.) _ 




a nd vv 

oi 


’ '"'"o’ ^nz 


are defined in cqueitions 





(A-68), (A-^95), (A-87), and (A-88) respectively • 

Finally, equations (A- 35) and (A- 38) lead, to the linearized pitch 



equation: 
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cos 9c) + (,Iy„— M^) ©e + u 

^ W = ( /cos ej Z,,- 0^ <A-'08) 

■*■ ®e I^St ©c, 'Sfe MaSs ■" f^Au ^ ^ 

- w; cose^ Wg + 



where 






AW 






v>/ 



(A- 109 ) 



Maw = 2 + C,, M„,^| J ^ z M_ w. 

+ (M^ + C,,, cos©^ 

M^^-tan©c 

Ml^mg = (^frU„- X^„w„)/ g 

MAee’' 2 g B) cos©^ 



(A- 113 ) 
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(A-U4) 

“'^o'^Av, 0 tan^ ©^j + CM^+ C,,, cos e^ 

^ ASs ^ '^ 4 . 1 ') ®<=- 

I^Aci = ■tan ©c 

(A-116) 

■^ '^o C^2 ,(m^,„i^|wJ * M^^COS©^) 

I^Amg= " Sm ©c <A-117) 



F„s. = “(z<^\a4-Zb B) sin©c * w„l]lw„l 

■^C,,, + w^ Cv,,M^.j) cos©e] 

+Cxj B- Xc,„'W^+ Iw^l + M^cose^) (A- 118 ) 

• COS 0^ 



and 



w 



w 



o o Ml 'Ic 

(A-87), and (A-88) respectively 



Cyjj and C are defined in equations (A-68), (A-95), 
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General 

In the linearization of the euqations of motion, equilibrium 
conditions were referred to. These are the values about which vari- 
ables are expanded in the linearization process. The manner of and 
the logic behind the choice of these conditions is dis cussed in this 
appendix. It should be understood that, unless stated otherwise herein, 
the equilibrium value of any variable is zero. It can be seen that all 
conditions evolve from the choice of ordered bubble and shaft rpm. 



2* Equilibrium Speeds 

In the actual operation of a submarine, equilibrium speeds during 
level flight are functions of equilibrium pitch and shaft rpm. For 
the computer simulations, shaft rpm was considered to remain constant 
to avoid the ambiguities which would have arisen in attempting to model 
the dynamics of the propulsion plant. The filter gains are predicated 
upon a value of u which is a function of shaft rpm. In the actual im- 
plementation of the filter, a provision for the input of shaft rpm should 

be sufficient to keep the filter updated for changes in u . 

c 

The evaluation of is necessarily one of the first steps in the 
simulations. The coefficients for the equations of motion are in di- 
mensional form. To calculate them, it is necessary to know the 

value of U . 

o 



The equilibrium speed problem is stated thusly: 
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Given: u and © 

c c 



Find: u , w , and U 

o o o 




Figure b-1. Representation of 
Eq\iilibrium Speeds 



Figure b-1 illustrates the relationships between most of the 
variables. They are: 

Uo=Uo/cos©^ (B-1) 

and 

Uo"tan ©c 



The solution of the speeds is begun with that of This is done 

via the linearized surge equation, equation (A- 92). In the equilibrium 
condition, assuming negligible external disturbances in surge, equation 
(A-92) reduces to 



0 = F,, 



(B-3) 
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which can be rearranged to read as follows; 

- [X _ - I ) X - (W„- B) sm e. 



where 




U, 



COS 0^ / 



U 



O 



Equation (B-4) is non-dimensionali zed by dividing by 

0= + a,) ui +b^u, u„ 

“ CWs” B) si n /-sr ^ 



l/2f>tX 



The solution of the "in trim weight (W^) follows (equation B-14). As 
is illustrated in figure G , is a function of velocity squared. 

Capitalizing upon this and the linear relationships between the velo- 
cities, it is possible to turn equation (B-6) into something resembling 

a quadratic equation for u : 

o 



-^a;. (Z'[^J-tan ©^1 + Z'l^)-ban©e 



C wiwi ^''11 NA/\\A/|'^ ^ ^ *tan C3c- 1 ww art 

izan^ 



(B-4) 



(B-5) 



(B-6) 



(B-7) 
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where is defined by equation (A-87). Due to , equation 

(B-7) is not actually quadratic, but it does lend itself to a iterative 
solution. Fortuitously, for the coefficients used in the numerical 
example herein, equation (B-7) actually is a quadratic equation: 

A Uo + C = O (b-8) 

where 

A'= Zl„-tane,+X^^)-tan^0<_ 

(Z. w, -tan ©j, + Z^,„i -tan^ ©J Kan \ (b-9) 

Z^~ban 

3 "= (B-IO) 



and 



C^= -+-El(F^^^^)-t:an0c/F ■ (B-ii) 

There are two solutions to equation (B-8), only one of which is real- 
istic. This turns out to be 

Uo 



(B-12) 
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for the coefficients in the numerical example. 



Having solved for u^, equations (B-1) and (B-2) are used to 
determine U and w . 



o 



o 



3. '^In Trim** Condition 

The equilibrium trim problem can be stated as follows: 

Given: u and © 

c c 

Find: W and 

o Go 

This is accomplished via the linearized heave and pitch equations, 
equations (A- 99) and (A-108) respectively. In equilibrium, the heave 
equation simplifies to 



It should be noted that the bouyant force , B, is assumed to remain 




(B.13) 



This equation is manipulated to find W^: 




c 




(B-14) 



constant and is equal to the weight of the water displaced by the volume 
of the entire submarine, including the free flooding volume. Therefore 
all changes in the trim of the submarine will be due to changes in the 
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density of the fluid in which the submarine is submerged, or due 
to changes of the mass within the envelope formed by the outer skin 
of the submarine. 

Having determined the linearized pitch equation is used 

to determine the longitudinal position of the CG for the ’in trim" 
condition. In equilibrium, equation (A-108) simplifies to 



0= F 4- E(F 

^ Ms V > MeK-t/ 

Rearranging this equation to solve for x 



Go’ 



(B-15) 



(B-16) 

-(z^\A4- ZaB)-tan©^ +E(F^^^^^)/cos,e^ 

'*cos,°e^ (fIwiwFC.,,1 



4. Deviations from ” in Trim Condition 

The following equations are repeated for clarity. 

W = Wq 



(A-56) 






><G-o 



X 



&e 



(A-60) 



The means of calculating W and are equations (B-14) and 

o Go 

(B-16). For the simulations, the submarine was put in an "out of trim" 



state by adding weights at various longitudinal locations. Hence, 



^ added 



w = z: w. 

^ 1=1 ^ 



(B-17) 




(B-18) 



The purpose of the trim filter is to make a quantitative estimate 
of these variables. 

5. Changes in Trim while Flooding Pumping 

Most of the simulations were conducted for step inputs in trim 
errors. A more realistic case is that of flooding or pumping. The 
following equations describe the effect of flooding at a longitudinal 
location designated x^. The rate of change of weight error is 




(B^19) 



At any instant, the CG is located at 



X 




(B-20) 



G- 




Differentiating equation (B-20) with respect to time yields 
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where 

W = W; 



and 




Pumping is 









(B-22) 



(Eriri-t 







(B-23) 



simulated by making W 



f 



less than zero. 
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Appendix C 



Controller Design 
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The general problem of the controller design was stated in section 3 
of chapter II. The linear system is described by 



d/ dtx = A x+B u 

—c — C — C — C “C 



(C-1) 



and the purpose of the controller is to minimize the performance index 



PI 



■ fe 9 



Q X + u P u )dt 
c — c c — c *— c 



(C<2) 



where both Q and P are symmetric, Q is positive semi- definite , 

— c — c c 

and P is positive definite. Physically this means that not all state 
*"c 

variables need be controlled and that all control variables must be 
weighted to preclude an infinite control effort. The optimal control is the 
feedback of the state: 



u 



= -K X 
— c— c 



(C-3) 



where 

T 

K = P B R (C-4) 

— c — c — c — 

and ^ is the solution to the matrix Riccati Equation (Appendix E). 

Concerning the weighting matrices, it is not their absolute magni- 
tudes that matter so much as their relative magnitude which determines 

the magnitude of the control effort which is proportional to Q and 

— c 

inversely proportional to P . Also, the relative magnitude of the 

c 

individual elements within each matrix determines the relative importance 
of each of the variables. When the steady state solution of the Riccati 
equation is used to calculate optimal feedback gains, the controller is 
classified as a linear regulator. 
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Appendix D 



Filter Design 
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1 • Optimal Filter 

The filter problem was set forth in section 4 of chapter II. The 
filter equation is 



d/dt If) 



(D.l) 



where 



-1 



Kf = ^ 9 . f f 



(D-2) 



and R is the solution to the matrix Riccati equation (Appendix E) which 
minimizes the performance index 



PI = 



= 1 






(D-3) 



Reference (7) gives excellent descriptions of the physical significance 
of the solution. The weighting matrices for the filter problem are not 
so arbitrary as those for the controller problem. For example, Q 



— f 



is the covariance matrix of the random system disturbances, w. 






E(w w"^) 



(D-4) 



For a two dimensional matrix 






E(w^ ) E(w^W2) 



ECWiW^) 






(D.5) 



which illustrates the diagonal elements of are the mean square 
values of the distrubances and the off-diagonal elements are indications 
of the cross correlation between the elements of w. 

Similarly, for the measurement noise, 

P - E(v v"^) (D-6) 



& 
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Physically, the solution of the Riccati equation is the error 
covariance matrix for the filter, 



R = E{x x^) 



(D-7) 



The Riccati equation for the filter problem is 



k ^ -f-f^f 




(D-8) 



The first two terms on the right hand side of equation (D-8) 
result from the unforced system characteristics in the absense of 



uncertainty due to measurements and their quality. 

Looking back at equation (D-2) it can be seen that the filter feedback 
gains are proportional to the uncertainty in the estimate and inversely 
proportional to the measurement noise. 

One other note: when the steady state solution to the Riccati 

equation is used to determine filter gains, the filter is classified as a 
Wiener filter. 

2 . Sub-Optimal Filter 

When the Riccati equation cannot be solved, some other means of 
determining filter feedback gains must be used. Looking at the filter 
equation in the absense of noise, 



measurements . ^ £ 

due to system noise and 



accounts for the increase in uncertainty 
T -1 

- R^£ P^ R accounts for the decrease in 



d/ dt X 



Ax + Bu - KCx 



(D-9) 



and 



X 



X 



X 



(D-10) 



is the estimate error. 
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The system equation is 

d/dtx-Ax+Bu 



(D-11) 



So if equation (D-11) is subtracted from (D-9), the result is 
d/dt "x = (A - ^ 



(D-12) 



which shows that the filter will be stable provided x goes to zero as 
time increases. Stated in another manner, the filter will be stable, 
provided the roots of the filter’s characteristic equation 

det(XI-A + KC) = 0 (D-13) 



have negative real parts. 

As mentioned earlier, it was necessary to select only the fifth 
and sixth rows of the gains matrix this way, the other gains having 
been found by Potter's method (Appendix E). It was possible to logically 
determine the signs of the gains based upon the qualitative behavior of 
the filter as described by equation (D-9). For example, if z^^ and/or 
were less than estimated, implying 

z >0 and/ or z > 0 

oe oe V ^ 



it would imply that the weight error (W ) was less than previously 
estimated. To correct this error, it would be necessary 



decrease 
A 

w 



A 

W , so 
e 

if k._ > 0 and 




0 



Due to the coupling between pitch and heave, the same phenomenon 
might be due to pitch being greater than estimated which in turn was 



94 



due to x_ being further aft than estimated, and 
Ge 

*Ge ♦ "51 ^ ’ 0 

Now if © and/or 0 were less than estimated, ie., 
e e 

© > 0 and/ or © "^ 0 

e 



it might be due to x 



Ge 



being further forward than estimated and 



"Ge' >' 53 ^ » ’'54^0 



Nothing conclusive can be said about W in this case unless x_ is 
® e Ge 

known, so both k . ^ and k. ^ were set equal to zero. These results 
63 64 ^ 

are summarized in Table d-1. 



k^, > 0 


k ^ > 0 


k ^ < 0 


k <0 


51 


52 


53 


54 


‘^61 ^ 0 




"63 


64 



Table d-1. Signs of Unknown Gains 

Using the constraints in the table, gains were selected at random 
and the eigenvalues of equation (D-13) were calculated. Then the gains 
were varied individually and in pairs to determine the sensitivity of 
the filter’s poles (eigenvalues) to each of the gains. In this manner, a 
sub-optimum was selected. 

It might be added that the decision concerning k and k being 

63 64 

zero was adhered to after discovering that comparable gains of either 
sign had very little effect on the poles of the filter. 



i 














I: 
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3 . Smoothing 

The filtered estimates of W and tended to reflect the noise in 

e Ge 

the system and measurements. The estimates were smoothed using 
the least squares algorithm in reference (9). 

The ’’best fif curve for a number of points is assumed to have 
the form 

X mt + b (D-14) 

It is the best curve in that it minimizes the squares of the deviations, 

d. X. - (mt. + b) (D-15) 

111 



, of the data points from the line, 
centroid of the n data points, 

_ n 

X = i y X. 

n 4 - — 1 

n 



The line will pass through the 



(D-16) 



and 



1 n 



t = - 7 t 



(D-17) 



The slope of the line wdll be 

r\ 



m 






x) (t. - t ) 



n 

I 



X) 



(D-18) 



so that equation (D-14) becomes 

X m(t - t ) + X 

and the standard deviation of the data about the line is 

7 - ) [It -^)] - [ % (x 



Vi 



n 



1 1 )^ 



(D-19) 
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This algorithm was implemented in a subroutine named LESQR 
(Appendix F) . 

This could be used for the trim analysis technique mentioned in 
chapter I, The plane deflections and pitch angle could be smoothed 
while the submarine stayed in the vicinity of ordered depth. Pre- 
sumably slopes would be zero and the results would merely be the averaged 
values of the angles which could be substituted into the linearized heave 
and pitch equations (equations A- 99 and A-108). These would then be 
solved algebraically to determine the state of the trim. The simplified 
equations are 



O-Z.^^Uo0e+ COS^ 'it 

and 

O = ©e 

— Wo cos ©o -i- 



(D-20) 



(D-21) 



Of course, W is based upon knowledge of the mean external disturbance 
o 

in heave (equation B- 14). If this is not known, the value of W for 

o 

no mean heave distrubance could be used without serious effects. W 

e 

could be driven to zero based upon equation (D-20) without knowledge 
of external disturbances. Then equation (D-21) would indicate the sign 
of closely approximate its magnitude. The true solution would 

be converged upon as the submarine was trimmed. 
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Appendix E 



The Matrix Riccati Equation 
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1 . Ge neral 

Calculation of the feedback gains for the optinnal controller and the 
Kalman filter requires the solution of the matrix Riccati equation. 

The equation is the solution to the following problem. 

Given: d/dt x(t) - A x(t) -f B uft) (E-1) 

Find: u(t) to minimize the quadratic performance index 

PI = ( (x^^ ^ hT (E - 2 ) 

''o 

where Q is positive definite and P is at least positive semi-definite . 
Both Q and P are symmetric. 

Using calculus of variations, the solution is found to be 

u{x, t) = -K (t) x(t) (E-3) 



where 

K(t) = P'^ R(t) 



(E-4) 



and R(t)is the solution to the nonlinear differential equation 

R(t) + Q - R(t)B R(t) + R(t) A + A'^R(t) = O (E-5) 

subject to the boundary condition 

R(T) = constant (E-6) 



Equation (E-5) is the matrix Riccati equation. Reference (5) is 
one of many references giving excellent detailed descriptions of the 
derivation of the Riccati equation. Readers so inclined are referred 
to those references. 

The requirement of this thesis is the solution of the Riccati 



equation as t goes to infinity. The principal techniques for the 
solution follow. 



99 



2 . Integration Method 

The most obvious method of solving the matrix Riccati equation is 
by integration of the n simultaneous nonlinear differential equations 
from the boundary condition to a steady state value. 

For the controller problem, the integration is performed backwards 
from the boundary condition, c>o ) ~ ^ . Whereas for the filter 

problem, the integration is performed forward in time from R(0) = 



The subroutine named MRS (Appendix F) is capable of this integration. 
3 . Eigenvector Decomposition 

This method is commonly referred to as Potter’s method. It yields 
the steady state solution of the matrix Riccati equation (reduced Riccati 
equation), provided the coefficient matrices are constant, ie . , 



The solution is found in terms of the eigenvectors of the 2n X 2n 
partitioned matrix 



E 




Q + R A + R - R B P"^ b'^R = O 



(E-7) 



A 



T 



Q 



M 




(E-8) 



The eigenvalues and corresponding eigenvectors of M are found: 




X.+ I o 



(E-9) 



where the subscripts denote the signs of the real parts of the eigenvalues 
and the corresponding eigenvectors are 



I 
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-T + 


— T- 




1 






-B + 


-B- 



(E-10) 



V is partitioned and the solution to the proUem is 






V. 



.-1 
“B + 



(E-11) 



Reference (5) contains the proof of the method. The eigenvalue/ 
eigenvector decomposition was performed using EISPAC (Appendix 
F) and these results were manipulated by the subroutine named MR A 
(Appendix F) . 



4. Controller/Filter Duality 

As has been previously implied, the optimal controller and Kalman 
filter problems are dual problems. Table e-1 illustrates the duality. 



lOl 




Table e-1. Optimal Controller/Kalman Filter Duality 
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Appendix F 
Computer Programs 
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A listing of some of the computer programs used in this thesis 
appear herein. The programming language was FORTRAN and 
comments are interspersed for the reader's benefit. Due to the 
magnitude of the packages, ACCESS II and EISPAC are not listed. 

ACCESS II is a collection of engineering application programs in 
the MIT Alechanical Engineering/ Civil Engineering joint Computer 
Facility. It is an updated version of an earlier package, and is pri- 
marily the work of Professor Richard S. Sidell, Instructor in Alechanical 
Engineering. ACCESS II is run on an INTER DATA AI70 computer. 

The following portions of the package were used for this thesis: 



CONTROL 


Forms a partitioned matrix like that on 
page 22. and determines the rank of 
the matrix to establish system control- 
lability or observability. 



EIGENATALUES Computes the eigenvalues of a system 





matrix. 


MR A 


Computes the steady state solution to the 
matrLx Riccati equation using Potter's 
method, and 


EISPAC 


Calculates complex eigenvalues and 
eigenvectors of a general real matrix. 

It was recently incorporated with ACCESS 
II as a subroutine for AIR A. EISPAC 
is taken from EISPACK, an eigensystem 
problem solver package developed through 
a National Science Foundation project 
known as NATS (National Activity to Test 
Software). The subroutines were tested 
principally at Argonne National Lab- 
oratory, the University of Texas, and 
Stanford University. 



The program and associated subroutines for the computer simulations 
is listed herein. Subroutines called but not listed are described 
in reference (10) and are: 



MINV 


Inverts a square matrix, and 


GAUSS 


Generates a random, normally 
distributed variable 
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RKGS Uses the Runge-Kutta method to 

obtain an approximate solution to a 
set of first order ordinary differential 
equations. 



Two other subroutines, MRS and MR DOT, are listed. These 
were taken from ACCESS II and modified for the IBM 360 used by the 
Author. These subroutines calculate the steady state solution of the 
matrix Riccati equation using the integration technique. 
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MAIN 



. tl -i. O 1, ^ X 



’i.MN P? 0.1? AM W5IICH 
lilTI'L CC.'.'DITICS 



IS USED TO SET UP THE 
FOP THE CO.'IPU'TEF S I ^iUL AT IONS 



DIMENSION ■' ESI ('M , V EC2 ( .’) , V {Q, 5) , W (-> , o ) , Z7 (9 , 3 ) , 7.CO ( 1U) , 

1 vco(6) , A''^:{6) ,xowT(-) ,x(is) ,?::•.!{ ") ,aux(9,15) 

COMMON DI-T''N (3) ,A (9, , 0 {“ ,2) , BFO? (O , 3 ^ , K {6 , P ) , Dr IN ( 2) , 

1 DEFMAX (2) , or MI N (2) , D3M AX (2) ,KI , KO ,CG ' IN (2 , , S'l'^PLE, 

2 I?:- I NT, (2) , FOP (3) ,:-OPSYS (3) , DTIMZV (3) , IDin, 

3 AF(6,5) ,pr(o,5) ,UT(5) . CCHEEC (B ) , ESEP (6) , TPPINT 

U , IIS TEP , XO?n, IX, noise (7) , SEN DF (3) , STNPM (71 , XXiSn (7) 
3,M0G,XG? ,XS0,r LE:AT,FLD^,':STAPT ,TSTOP,TVEC (3 0) , XV EC (30) , 

5 ;iV2C(30) 



HEAL MASSP, nowin?, 



, MOG, :NC,UBOW , XD0,riD''', MQ, NQN, MS , 



1 MS N, MSI AH , MX , X ) '-(1 , MW N, Ml Wl 0 ,MWW, MW1 W’1 , « U 1 W1 N , MDELDW , 

2 MEELW,i-lIN?.P,XC^' (13) , K , MGE , NOIS E 



SXTEFNAL FCT , OUT?, L ESQH 
KI = 5 



EO = 6 

GRAV=32, 1723 
BAC=57. 2 9377Q51 3 



DO 2010 1=1,9 
DO 2000 J=1,9 
V (I, J) =0. 

W (I, J) = 0. 

>00 COKIINOE 

DO 2005 J=1 , 3 
105 ZZ (I, J) =0.0 
DO 2010 J=1,2 
B(I, J) =0.0 
'10 CONTINUE 
B(5, 1) =1.0 
B(6,2) =1.0 
V(1, 1) =1. 

V(3,3) = 1. 

V(5,5) =1 . 

V (6, 5) =1 . 

V (7,7) =1 ,0 
V(6,8) =1.0 
V (9, 9) =1 .0 
W(1,2)=1. 

W(3,t) — I. 

ZZ ' 

ZZ (C, 2) =1 . 

ZZ (7, 3) =1 . 3 

'HAD IDENTITY ’'ND NO NDI MF N SION AL IZED C0EFFICIFNT3 
FEAD(FI,100 0lID,LN,w/\ssp^KIv^p.p^XBP,XGr,Z3F,ZGP 
READ (KI , 1 007) {7CC(1) ,1=1,19) 

FEAD (YI, 130 3) (MCC(I) ,1=1,1"^) 

FEAD (KI, 100 3) (XCC (I) ,I = " , 3) 

^EA? MEAN DISTURBANCES AND STANDAPr DEVIATIONS OF SYSTEM 
IISTU'^EANCES AND NEASUFENENT NCISES 



MAIN 



?z?D 1005) (PIS': .'It. (I) 3) 

FZAD (KI , 100') (Srt.DF(I) ,1=1,^) 

R2AD (KI, 100"’) /Srt’rM(I) ,1=1,'’) 

13 FEADf'r'I, 1 00 3) UC,3aEELE, DENS 
PEAD (KI, 1 1 OO) N'A'^"'.AS 

'EAD tIAGtIir'JDF (IBS) AND LCG(FT) OF ADDED WEIGHTS 
"lEAD (KI, 1002) ( ADWT (I) ,XGyT (I) , 1= 1 , NA DX, A S ) 

3EAD CCNTPCLLEP G'IN’S 

FEAD (KI, 10 06) { (CGAIN (I,J) ,J = 1, 6) ,1 = 1,2) 

READ FILTEF GAINS 

FEAD (K I, 10 04) ( (K (I, J) ,0 = 1 ,E) ,1=1,6) 

1SAD CG’JSIFAINrS ON PLANES 

LEAD (KI, ICON) (Dr:F;-lIN (I) , DEFF! AX ( I) ,1=1,2) 

READ (KI , 1 00.-) (DFXIN(I) ,DFTAX(I) 2) 

WPir E (KO, 32 ' 4) I-:, uC, D'JBBLE, DENS 
WRITE(KG,3215) (DIsrXN(I) ,1=1,3) 
rONVERT V^LOCiriFS F'CtI KNOTS TO FT/SEC 
UC=IIC*1 .63306^ 

THETA=3'J33LE/F AD 
TARI = TAN (THETA) 

TANT2 = TANT’»'T ANT 
TANI3=T ANT2-TAFT 
SIM=SIN (THETA) 

COST = COS (THETA) 

COST2=COST*CCST 
C2=0. 5*DEN5*LN'«': N 
C3=O.5*D£NS*LN**5.0 
C2i = 0.5*DEN3-lN*’^-i.0 
C5=0. 5*DEN3 . 0 

ESTIMATE 00 3ASED UPON UC AND lTBBLE 

AA=XCO (5) +XCO ( 1) +TANT2* (ZCC (0) +TANT-^ZCO (12) +XC 
1 AES (TANT) * (''AN T’^^ZCC (1 0) t-TANTZ+ZCO ( 1 3) ) + 1 A NT 
E5=UC*XCO (2) 

cc=uc*uc*xco (3) 4-: ant^distxn ( 1) /C2 

FOCT=BB*E3-4.0*AA*CC 

UO= (-EB-S03T (ROOT) ) / (2. 0*A A) 

KO=CO*TANT 
ABSWO = ABS ( rtC) 

VEL=yO/COST 
VSO=VEL*VF 1 

W'?ITE(KO,372-,) Ur,!:0,VEl,WG 
CN2=-UC*CO 37/UC/UO 
CNi = -CN2-MO-'' . 0 
AIM E NS 10 N A LI Z £ CO F FFI C I E NT S 
&0 = C3*XASS?*GF A V 
DISP=BO/2240. 
fiOMINR = C5*NINRP 
X3=Y 3P*1N 
XG=XGP*LN 
ZB=ZBP*IN 



n o 
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ZG=ZGP*LN 

ZBCH = C2 *vs c)*7;go <' ) 

700 = 0^*7,00 (2) 

7DV=C3*ZCO (3) 

Z0=C3*V EL*Z00 (ii) 

70N=ZC0 (5) *C3*V0L 
Z3 = C2*V50*ZCO 
ZSN=C2*ySQ*Z0C (7) 

ZS':AR=C2*V30*ZCC {^) 

ZW=C2*VEL*ZC0 (9) 

Z1 Vl = C2*V£L*ZC0 ( 10) 

ZWN=C2*VEL*Z00 (11) 

ZWK=C2*ZCO(12) 

ZV1W1 = C2*ZC0 (1 3) 

ZWlvnN = C2*ZC0 (19) 

KBOW=C3 *VSO'*''1CO (1) 

KDC=C5*MCO (2) 

:iDW=C4*«C0 (3) 

MQ=CC*V £L*M00 (9) 

KON=C4*VEL*7.CO (5) 

«S = C3*VSO*''iCO (6) 

«SN=C3*VSQ*MC0 (7) 

MSTAa=C3*VSQ*MC0 (3) 

HW=C3*VEL*.'1C0 (Q) 

K1W''=C3*VE1*MC0 (10) 

MWN = C3*VEL*'1CC (11) 

M1W1Q=CU*?'.00(12) 

MWVi = C3»J1C0 (13) 

^W1W1=C3*7.CC ( 14) 
awl W1N = C 3*MC0 (1 5) 

AI=C2*XC0 ( 1 ) 

BI=C2*XC0 (2) 

CI=C2*XC0 (3) 

XDU=C3*XC0 (4) 

X0U=C2*XCC (5) 

XMC=C3*X00 (6) 

XWK = C2*XCC (7) 

XWWN=C2*'^C0 (3) 

ALCULA'^E THE PROPER TPIX 

aOG=BO-Z 17 1 *A E3 70- CO SO* 7 STAR- WO* ( ABSWO* (ZW 1 W 1 *CS1 * 

1 ZW1 WIN) WO*ZW W>COST* (Z W+CM1 *ZWN) ) /COST-DISOaN ( 1 ) /COST 
WrMBO = .aOG-BO 
KO=aOG/G RAV 

XG0aaG=X3*B0+ai W1 *ABSW0*-C0ST*MSTAH-TANT* (ZG**!0G-ZB*D0) + 
1W0* ( ABSWO* ( XW 1 W UCN 1 *1W 1 W 1 N) +00 ST* (MW + CN 1 *X WN) +WO*MWW) / 
ZCOST + DISTX:! (2) /COSO 
XG0f10=XG0X0G/GRAV 
XGC=XG0a0/.'10 

W'RTTS (KO ,3 371 ) DISP , WTMBO , XGO 
ALCrjLATE SRROES IN TRIM 
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XG^!G = XGO^lOG 
'1GE=0. 0 

DO U1 I=1,'JADKAS 
KGE=MGE+ \ C-T ( I) 

XGXG=XGf;G4-XGV.r (I) ^'ADV.T (I) 

wght=xog+xgz 

xg?.ct=xg:- g/'a’ght 

XG£=XGACr-XGO 
WRITE (KO,33"’2) •'Gi',XGE 
ALCULATE STEADY STATE FORCES 

FOFSYS (1) =COST=*^ (:-'OG-BO + Z1 W i * A 3S VO + COST *ZST AP ) *-V.O* (ABSWO* 

■' (ZWI W 1 +CM 1 *ZW 1 ',’1 S-) +WO+Z'wW + CJST* (Z W + C?!1 * ZW !.') ) 

FOFSYS (2) =-SIN fZG*'-.OG-ZB*3 D) ♦•WO’fr (ABSWO* (/:W1W 1 +CN’ 1 <= X W1 W1 N ) 
2+CCST* (XW + CN'1 «^:rJN) +v;0*XWw) ‘•cost* (XB*BO-XGOnOG-i-Mlw1 * 

3 AESWO + COST*''S'“M) 

FOFSYS (3) =WO*WO* ( XWW-‘CN 1 *yv;?N) <-00* (UO* (AI + XUU) +BI*t;c) * 

1 CI*SC*(JC-3 INT* (?;OG-BO> 

ALCULATS ELEMENTS OF THE MATFICE3 

ZDELW = 2 , 0*ABS WC* (ZWIKI -t-CF *Z W 1 W 1 N) ■•■2 , 0 FW *W 0+ 

1CQST* (zw + c:n *z«'N) 

ZDELDW=XO-ZDW 

D £ L W = 2 . 0 * A 2 5 W 0 * ( X y 1 W 1 C N 1 * M W 1 W 1 N ) * 2 . 0 * E W V * W 0 
1CCST* (M*'+C'J1*X'--''n 
X D E L D W = - X G 0 X C - X D V 
XDELW = 2. 0*WO* ('^Wv. 4-CN'1*XWWI:) 

V (2, 2) =ZDELDW/COST 

V (2, 4) =-XGOXD-ZD0 
v(2,7)=zdeldw*t.\f;t 
7 {2, 9) =WO/GHAV 

V (4,2) = XDELDW/CDSr 

V (4, a) =XOXIN'R-XDs: 

V (4,7)=ZG*X0-t-X'^ElD».*TANT 
7(4,9) = (ZG*90- XGQ*WO) /GPA7 

V (7, 4) =XC*ZG 

V (7,7) =*0-XCS 
7(7,9) =nC/G?A V 

W (2,2) =ZDELW/COST 
K(2, 3) =ZDELW*nO 

(2, 4) =C0ST* (Z ;*TN1*Z0N) +UG* (XO-ZDELDV* (1.0 TANT2) ) 

W (2,5) =C0ST2*Z'>0V; 

4(2,6) =C0ST2* (Z5 ‘■CN'! *Z3N’) 

W (2,7)=W0*CN2* (Z'« 1W 1 N'*ABSyO+COST*ZWN’) ‘•ZDE1W*TANT 
W (2, 9) =COS 1 
W (4,2) =XDELW/COST 

W (4, 3) =U0*XDEI -i-COST* ( ZG *XOG -Z E *B0) 

4 (4,4) =X0* (-XGG*!IG-ZG*WG) -•:3*XDELDW* M .O+TAZTI) + 

A Ml W1 g*A3SyO-^COST* ( XC ‘■CN 1 *X0':) 

W (4, 5) =X304*C0S:2 
W (4 ,6) =C0ST2* (X5+CN’1 *XS4) 

W ( '4 , 7) =TAN T*'' DED4+ W0*CN2* (X'-'l Wl N*AESWC + XWR*COS I) 
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w (a , 3) =-7.oi*cos” 

* 9) =-XG J*C0 3T-ZG*SINT 

W (7 , 2) ■=XDELW/cr;3': 

W (7,3) =70«'XDFL«- C10G-D0) *Cf'ST 
W (7,U) ='4C* { XWO-’^F!) 

(7,7) =2. 3 *'JO* { AJ + XUU) +3I*';C*-CH2*XN'fN* V04:^OfXDElW<=TMiT 
)| (7. 9) =*-SIN'r 

,ODI?Y AATE'CES FOF Vi'GLES IS lEGHZSS 
DO 377 1=3,0 

V (2,1) =/ (2, i) /. . 

W(2,I) - '■ .1) 

V (t , I) =V : . , I) /FAD 
wr-i,I)=W(a,l) /?AD 

V (7, 1) =v (■', I) /:;ad 
77 W(7,I)=W (7,1) /PAD 

WRITE (KO, 3098) 

WRITE C/0, '1 1C) ( (V(I,J) , J=1 ,9) ,1 = 1 ,^^) 

WRITS (KO, 3 3 99) 

WRITS(KO,3110) ( (X(I,J) , J=1 ,9) ,1 = 1 ,9) 

CAIL MIN’V (V ,9 , DET EPn, VEC1 ,\’zC2) 

CALL :'P5D(V,W, A.,c,9,9) 

OOIFY A MATPIX FOP XGE IS XEGAPOONDS 
DO 54 I = ‘',9 

5- A(I,9)=A (1,9) »1000000. 

OMVERT TF.Ifl IF ?0F 2^ KEGAPOUSCS 
MGE = aGE/‘' 000000. 

WRITE (KO, 30 22) 

W3ITE(KO,3110) ((A(I,J) ,J=-',9) ,1 = 1,9) 

CALL .1??D(V,ZZ,BFOF,9,9,3) 

WHITS («^0, 3026) 

WHITE (KO, 3 1 30) ( (BFOF (I , J) , J = 1 ,3) , 1=1 ,^) 

WRITS (KO, 3 102) (-DFSYS (I) ,1 = 1,3) 

WHITE (KO, 22 22) 

WHITE (KO, 2223) ( (CGAIN (I ,J) , J = 1 ,6) , 1=1 ,2) 

WHITE (KO, 2224) 

WHITE (KO,2223) { C-' (I, J) , J= 1 , ^ ) , 1 = 1 , 6) 

RGANIZE 033EFVE? A K B XATHIX D INITIAL' DF . 

DO 230-k 1 = 1, w 
DO 2301 J=1,4 

01 AF (I ,J) =A (I , J) 

DO 2302 J=5,6 
jKL=j-4 

DF fl, JM4) =A (I, J) 

JP3=J+3 

02 A? (I, J) =A (I, JP3) 

DO 2303 J=1,2 
JP2=J+2 

03 OF (I , JP2) =3FOr (I , J) 

04 BF (I ,5) =A (1,7) 

DO 2306 1=5,6 



I 
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DO 2 30 5 J=1 ,6 

05 ?.?(I,J)=0.0 
DO 2306 J=1,5 

06 ?F (I , J) =0.0 
WRITS (KO, 302 3) 

WHITS (KO, 2 22 6) MAF (I , J) , J = 1 ,6) ,I-=1 ,6) 

WHITS (KO, 3 100) 

white (KO, 2 227) ( (bf (I , J) , J=1 ,5) ,1=1,6) 

ST ('? FOP T!!S OFD. TIFF. EQN. SOLN. 

DO 215 1=1,3 
DTIESV (I) =0.0 

15 PCF (I) =FO?SYS (I) +DI5I.1N (I) +DTI1EV (I) 
P3AD(KI,100H) (X(T),I = 1,15), XOP.D 
U (1) =0. 0 
U (2) =0.0 
U? (1) =X (-) 

CF (2) =X (5) 

UF (3)=FO?3YS (1) 

UF («) =F0?SY3 (2) 

OF (5) =X (7) 

X (8) =XGE 
X (9) =FGZ 

ET U? VECTORS FOB LFAST 3QUAEE SMOOTHEE 
DO 352 1=2,30 
XVFC (I) =X (1 
W7EC (I) =X (15) 

52 TVFC (I) =-62. + 2.0*FICA: (I) 

ALCOLATS THE SR30BS IN THE ESTISATES 
ESF? (1) =X (10) -X (1) 

E3E? (2) =X (V) -X 12) 

■SEP (3) =X (12) -X (3) 

E3EH (U) =X (1 3) -X (C) 

E5FP (5) =v (13) -X (H) 

ESER (6) = X (13) -X (''0 

ALCOLATE THE COPF.ECTIONS FOB THE rlllE? ECUATIONS 
DO 65 1=1,6 
COPREC (I) =0. 0 
DO u5 J=1,3 

65 COFPSC (I) =COFPEC (I) -K (I , J) *23SE (J) 

(1 ) =INITIAL TIFF 
HMT (2) =FINAL TIME 
3MT (3) = I’!ITIAL TIME INCFSMENI 
Bi'iT (4) =EEBCH BOUND 

BEAD (KI, 1500) (P?XT(I) ,l = 1,ii) ,ITSTSP 
WHITE (‘fQ ,?006) (H-\TF(I) ,1=1, 3) 

WSITE(KO,2006) (3:nDM(I) ,1=1,7) 

WHITS (KO, 2723) (Pr'MT(I) ,I=2,-i) 

KITIALLY DEFX IS '^FLATIVS INPUT SFPOB WEIGHTS 
HEAD (KI , 1008) (DI-.<(I) ,1 = 1,15) 

SAD FLOODING CASUALTY FABAMETEH5 
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:^I]AD (KI, 100 U) FLD3 AT, XGF,T START, TSTOP 

IX=3479 

FLDP=0. 

I?FI NT=0 

T??INT=0-0 

iDiD=n 

CALL TKAS ( T ~ H T , X , D El- X , 1 5 , 1 H LF , FCT , OOT ?, AU X) 

300 FOFF. AT (I 10,7F1 0. 3) 

302 F3FMAT (2F10. 3) 

303 FCFXAT ( AF10. 3) 

30 4 F3FXAT (4F1 .3. 3) 

305 FGPXAT (6F10. 3) 

307 FOFM AT r' F-'O. 3) 

303 F3FXAT (SF"' 0. 3) 

100 FOFXAT(IIO) 

500 FOFMAT(-F10. 3,3110) 

30- FOfMAT (1H0, * EXTEF.MAL DISTOP DANCE STANDARD DEVIATIONS:', 

I 1F3E1U.6) 

306 ^OF-.AT (1H0, • '■SP- NOISE STND. DE VI ATIONS : ’ , 1 ? 7F 1 4 . 6 ) 

III FORM AT (1 HI , 30X, 'CONTFOLLEF GAINS') 

223 FOF^.ATdH , z ( 1 P6 El 5. 6 , / , 1 X ) ) 

22u FORMAT ( 1H0, 24X , • FILTEF GAINS') 

225 FOFMATdH , d { 1 P6E1 5. 6 , /, ■> X) ) 

227 FOFMATdH , 6 (1 P5 Z1 u . 6 , / , 1 X ) ) 

225 FOFMATdH , d ( 1 P - F 1 5 . o , / , 1 X) ) 

723 FOFMAI dHO ,' FINAL TI HE= ' , F6 . 0 , ' SEC, TIME STEP=',F6.4, 
1', UPEE? ERF.OK FOa N D= ' , F6. 3 ) 

322 FORMAT (VJO, 30X, • A MATRIX {ANGLES IN DEGREES AND TRIM', 

1' ERROR IN MEGAPOUNDS AND FEET)') 

323 FORMAT (1H0, SOX, ' FILTER A MATRIX') 

026 FCFMAKIH ,10X,'EFO? MATRIX') 

39S FORMAT (1tiO,45X, ' V MATRIX (ANGLES IN DEGPEE3)') 

399 FORMAT d‘iO ,-5X, 'V MATRIX (ANGLES IN DFGREES)') 

100 FOFMAI (1H0, 25x, ' FILTEF 3 .'ATRIX') 

102 FOFMATdH , 'VEHICLE STEADY STATE FCFCES : ' , 1 PE 1 a . 6 , 

1' IBS, ' , £1 ^ ' FT-IBS, ' * IBS') 

110 FOFMATdH , 9 ( 1 P^- E 1 U . r , / , 1 X ) ) 

130 FOFMATdH , ^- ( 1 P 3 E 1 - . 6 , / , 1 X) ) 

214 FOF“ AT dHO, ' TD=' , 13, ' , TURNS FOR',F5.1,' KNOTS, ', 

1' CRDFRED 3H3F: '= ' , F^. 1 , ' DEGPEFS, D EN SI T Y= ' , F 7 . 4 ) 

215 FOF MAT (1 :i0, ' MEA;; disturbances A?E: ' , 1P3E15.6) 

371 FOFMAT d.HO, ' DIS’'TACEMENT=' , 1 PEI 3.6 , ' TONS, 2ZIGHT MINUS' 
)' DISPLACEMENT^ ' , E"' 3. 6, ' IBS, XGO= ' , E 1 3 . 5 , 33 FT) 

372 FOFMAK1HO, ' IF TM E?.ROR= ' , 1 PE 1 3 . 6 , ' LBS, ',E13.6,' FT. IN 

72'i FORMAT (IHO, 'VZLOCIIIES (FT/SEC) : UC= ' , 1 ?E 1 3 . 6 , 5H , UC = 

1,El3.c,FH, VLI=,El3.6,5li, Xj = ,El3.c) 

STOP 

END 
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fiAIN 



SnBPOUTINE MPPD(A, B, C, N P , N’K , NC) 
rHIS SUBFOOriNE rULTIPIIES CONFORMAL 
REAL A ( 1) , B (1) ,C (1) 

DO 20 1=1, NR 
DO 20 J=1,NC 
T=0.0D0 
DO 10 K=1,NK 
IK=I + NR* (K-1) 

KJ=K*NK-*' (J-1) 

) T=T*A (IK) *3 (KJ) 

IJ=I + NR* (J-1) 

) C(IJ)=T 
RETDF.N 
END 



MATRICES 



lESQB 




2 I??IN’:,U (2) , Foa r>) , F 

3 AF (5,6) ,BF(5,5) ,UF(5) , 



. J -L ^ \ , I , J. .J . , 

CORRBC(6) ,E?Z2(5) ,7?BINT 
DF (3) ,STND?! (7) , (7) 

ART,TSTOP,TVEC (30) ,XVSC(30) , 




5 ^VEC(30) 

REAL 

BOATS VECTORS CONTAINING DATA TO BE SMOOTHED 
DO 10 1=1,29 
IP1=I+1 

XVEC (I) =XVSC (I?1 ) 

«VEC (I) =WVEC (IP1) 

10 TVEC (I) =IV2C (I?1 ) 

XVEC (30) =XGEEST 
WVEC (30) =WEZST 
TVEC (30) =T 
SUMT=0. 0 
SUKX=0. 0 
SUKW=0. 0 
SOKT2 = 0. 0 
50KX2=0. 0 
SUMW2=0. 0 
S0KTX=0. 0 
SUKTW=0. 0 

CO 20 N=1,30 - , 

SOMT=SUMT+TVEC (N) 

SOMX = SUMX+XVEC fN) 

SOMV=SUMW* WVTC (N) 

SUMT2=SUMr2 + TV5T (N) *^2 
SUMX2 = SU’'X2 + XV EC (N) **2 
SOMN2 = SUMW2 + VVrC (N) ^*2 
SOMTX=SUMTX + TVSC (N) *XVEC (N) 

20 30MTV=SU''TW + TVEC (N) *WVEC (N) 

:m=sumt/3o. 

XM=S'JMY/30. 

ViM = SOM V/30. 

3IGT2 = S0XT2 - 30. 

3IGX2=S'JMX2-30. 



lESO 



SIGW2 = S[I?iy2- 30. 

SIGTX = S'J"' 7X-^0 . =^TX*vf.-. 
SIGTW-SO :-'7W - '>0. 
MX=SIGrX/SrGI2 
riy = SIGTW/SIGT2 
xgi;s=lXX* (2- IX) +X0 
yzs=aw* (T-TM) two 
P.ZTUF N’ 

END 



lib 



WAIN 



C H IS 
DIFT 



E 

su 

r?r: 

ui^ 



1 

o 

3 

5 

6 



POUT 
3F0U 
WTIA 
ENSI 
.ION 
F'lAX 

( 6 , ^ 

-r .-f O' 'T' *? n 

<r ^ ^ .1- ^ 



INS OniP ("J,X, DSPX,irLF,NDIM,PH.'iT) 

-I T vj U> -r < 



IS 



AT 



,;ic 

>J V 
ISA 



G , XG 
SC(3 
L K, 



L SJUA 
ON X ( 1 
Disr:-'N 
C) , 0" 
I? "INI 
) ,5? (6 
,XOSD, 
?,VGG, 
0 ) 

NOISE, 



:AILED between 'T’IXE steps of "HS 

‘NS SOLUTION 



?) , DEF.X ( 15) , ?E ( 5) 

i i) ,A(9,?) ,B (^,2) ,BFO?(9,3) ,K (6,0) ,DEFMIM(2) 
■'IN '2) , DEXAX (2) ,KI,KO,CGAIN (2,5) , 5M33LE, 
,U(2) ,FO?(3) ,FO?.SYS(3) ,DTI2E'M3) ,IPID, 

,') ,UF(5) , COF,?,EC(6) , ESSE (6) ,IPEINT 

I Y, noise (7) ,SIN'DF (3) ,SINDM (7) ,x.NSF '"') 
F10FAT,rLDF,TSrA5T,TSTCP,:’VEC (30) ,XVEC(30) , 



KOG 



^LS-NENTS OF THE X V ECTOF AF E AS FOLLOWS: 
C (1) =DZPTH 

( (2) ^EATS OF DESCENI 
( ( 3) ='’IICH EEFOF 
( (4) =IA'"E OF PITCH 
C (5) =‘''AI5WATEH FLAN’=' DEFLECTION 
C(o)=3TSFS PLANE DEFLECTION 
( (7) =FC':WAFD VELOCITY DEVIATION 
C(3)=XG EFROF 
( (9) =WEIGHT EFRCR 
C{10) = EE?TH ZSTIXATE 
C{11)=FAIE OF DESCENT ESTIMATE 
C(12)=PITCH EFROF ZSTIXATZ 
C(13)=FITCH FATE ESTIF.ATF 
C(1fi)=XG EPEOE ESTIMATE 
C(15)=WEIGHT SRFOH ZSTIXATE 



FLD?,=FLD?AT 

IF (T.IT. TSTAFT) ?LDR = 0. 

IF (T.GE. TSTCP) FLDR = 0. 



3 Z 


NERATZ 


SYSTEM AND iZASU 


?S 




EN'^ 


NOISE 






DO ^ 


29 1=1,3 














CAIL 


GAUSS (IX, SINDF (I 


) , 


0 


.0, 


DTIXSV (I) 


52 


9 


FOR ( 


I) =FO?5YS (I) ^DIST 


MN 


( 


I) + 


DTIKZV (I) 






DO 53ii 1=1,7 














CAIL 


GAUSS (IX, SINDM (I 


) , 


0 


.0, 


NOISE (I) ) 


)3 


!* 


XXSR 


(I) =X (I) +NOI£Z(I) 










)S 


PTH CH 


ANGZ ORDZFS 














IF (T 


. GT. 30. ) XO?0=180. 














IF (T 


.GT.60.) X0RD=210. 














IF f r 


. GT. 12 0.) X07D=180 














I? (I 


.GT. 200. ) XORD=21 0 














IF (T 


. GT.2P0.) yoRD=ieo 


• 








:a 


lcula: 


ION OF OPTIMAL CO 


liZ 


t: 


OL 








DC 7 


0 1=1,2 














U (I) 


= CGAIN (!,“')* (XYSF 


(1 


) 


-XO 


RD) 



( 
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DO 70 J=2,6 

7 0 UC) =U(I) +CGAI^MI, J) (J) 

:r?OSITIOK CF CCNSTFArNTF ON PLANES 
CO 50 1=1,2 
IDEF=C *1 

IF (U (I) ) 10 , 50, 30 
10 CO NX INUZ 





IF (X (lOFF) . 


T T 


DEF 


,i I N ( I 


) ) GO ro 




IF (U ( 


I) . GF. 


DPM 


IN ( 


I) ) GO 


TO 50 




0(1) = 


DR MIN ( 


I) 










GO TO 


50 










30 


CONTINUE 












IF (X ( 


IDE?) . 


GE. 


DE^^.AX {I 


) ) GO TO 




IF(U(I) .LF. 


r\ ^ ,v 

4./ ^ i. 


AX ( 


I) ) GO 


TO 50 




U(I) = 


DR MAX ( 


I) 










GO TO 


50 










47 


X (IDE 


F) =DE- 


V 


(I) 








GO TO 


49 










43 


X (IDE 


F) =DE? 


MAX (I) 






49 


0(1) = 


0. 










50 


CCNTI 


NUE 











1EA3UPE OF ?0? THE FILTER 



UF (1 ) =XNSR (5) 

UF (2) =X.'5S? (6) 

UF (5) =XMSR (7) 

lALCULATS THE APPARENT 8 HEAL ERRORS IN THE ESTIMATES 
ESER ( 1 ) =X (10) -XM3R (1) 

ESER (2) =X (1 '') -X 133 (2) 

ESFR (3) =X (12) SR (J) 

CSE= .) -••■VC- , 

ZSi: -V (o, 

ESEF (6) =X (15) -X O) 

:ALCULA"E the corrections for the filter equations 
DO U5 1=1,6 
COFREC (I) =0.0 
DO ti5 J = 1 ,4 

45 COFFEC (I) =COFFFC(I) -K (I, J) *FSEP (J) 
xsirci PRINTING OF THE OUTPUT 
IF (T.LT. TPRINT) GO TO 3 
I F F. I N T = I P ?. I N T ♦ 1 7 S T E P 
ITEMP=IHLF+ 1 

TPr INT=FLCAT (IPRINT) -PRMT (3) /FLOAT (ITEMP) 

SMOOTH ESTIMATE CF STATE OF TRIM 

CALL LESQR (T, X (1 4) , X (1 5) , XGES, WES) 

IFdDID.LT. “I) GO TO 8 
WRIT’S (KO ,2001 ) 

ICJD=0 



3 XPUB=X (3) +3npBLL 

WPITE(KO, 30 00) T,”(1) ,U(2) ,X(1) ,X(2) ,XBUB,X (4) , 
1 >; (8) , X (9) , IHLF 
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OUTP 



HRITS (KC,3001 ) X (7) 
'V?:7E (KO,3 00 " 



,X (5) ,X (O , (FSSR (I) , 
) , y ( 1 5) , XORD, (COPPFC 
:v?JTE(KO,3001) X^.SR (7) ,XMSP <:5) ,XM5? , 



1 ,XGES,W£S 
IDID=IDID+1 
3 COT'TIN'JE 

)G1 rOP'' \T ( 131 , P'S , • TIX 
’ ~‘-9, 'DEPTH' 
T'PITCH HATE', 7102, 
3 'IHLF' , /,T5, ' LETTA 
^ (7X, ' EST-H3F. ' ) ,2 
5 ' XGE E3T ' ,T1 ' , ’ HGE 

P 'CCTHEC ,5 ("V, • CC? 

7 2 (2X, ' SHOOTiiED I S 
)00 rOPE^T (1H0,E'^.3,' 
)01 FOFEATMH ,lP9E'!i;. 
SET OF N 
END 



E' , n 9 , ' Pf’ HATE' ,733 
,753 , ' DESCENT HATE' , 
'XG ERP.OP ' ,71 1 6 ,' EG 
U' ,71 9 , ' DP DEE. ' ,73 
(7X, ' ES7-AC7' ) ,/,T5, 
ES7 ' ,730 ,' ORDERED D 
PEC) ,/,:i,7 (3X, 'HEA 
7') ) 

SEC, ' ,1 ?6E14.6,I3) 

6 ) 



r= 1 , 

(I) 

(XXSP (. 



t X u ~ 

776, ' P. 
ERROR ' 

3 , ' S P 



r * 

DE 



V I 

“ / 

C';‘ • 

128 

I 



EP7H ' , 
S U F E H E 



NT 



8, 
’) , 






,788, 

# 

9 
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MAIN 



SOBEODTINE FCT (T , X , DER X) 

HIS SUBROUTINE CALCULATES THE RIGHT HAND SIDES OF THE 
iIFFERF.NTIAL EQUATIONS FOR THE RUNGE-KUTTA SOLUTION 
DIMENSION X ( 1 5) , DERX (1 5) 

COMMON DISTMN (3) , A (Q, 3) , B (°, 2) , BF03 (9, ,K {6, 6) ,DEFMIN (2) , 

1 DEFMAX (2) ,D^MIN (2) ,DRMAX(2) , KI , KO , CG A I N (2 , 6 ) , BUBBLE, 

2 IPRINT, U (2) , FOR (3) ,FORSYS {3 ) , DT I M E V (3 ) ,IDID, 

3 AF (6,6) , B?{6, 5) ,UF(5) , COPREC (6) ,ESE3 (5) ,TPRINT 

4 , ITSTEP, XORD , IX, NOISE (7) , SINDF (3) , STNDM (7) ,XM5 R (7) 
5,MOG,XGF,XGO, FLDRAT, f L D R , 1ST ART , TS TOP , TV EC ( 3 0) ,XVEC (30) , 

5 WVEC(30) 

REAL K, NOISE, MOG 
TATE EQUATIONS 
DO 30 1=1,9 
DSPX (I) =0. 

DO 10 J=1,9 

10 DEPX (I) =DERX (I) +A (I, J) *X (J) 

DO 20 J=1,2 

20 DEPX (I) =DZRX(I) +B(I,J) *U(J) 

DO 30 J=1,3 

30 DEPX (I) = DERX (I) +BFOR (I, J) *FOR (J) 

DERX (8) = FLOP* (XGF-XGO-X (8) ) /(MOG + X (9) *1000000. ) 

DERX (9) =FL DR/1 0 000 00. 

ILTER EQUATIONS 
DO 42 1=1,6 
IP9=I+9 

DERX (IP9) =CORREC (I) 

DO 4 1 J=1,5 
JP9=J+9 

4 1 DEPX (IP«) =DE?X (IP9) + AP (I, J) *X (JP9) 

DO 4 2 J=1 , 5 

4 2 DEPX (IP9) = DEP,X (IP9) ♦BF(T,J) *UF(J) 

RETURN 

END 
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SaP^OUTINE M?.S (A,B,QO,PO,EO,?,KT, DT,NFOW ,NCX,I?MT) 

THIS IS A 'lOOIEICATION OF THE SH3R0DTINS FF.OH THE ACCESS II 
PACKAGI AT yilZ 

COPYPIGHT H.I.T. 197U 

THE ARG'JHENTS ?0? THIS SUBROUTINE ARE AS FOLLOWS: 

A ANE E AFE COEFFICIENT MA'PICES 

QO IS THE Q «A:*RIX FROM THE PEPFOR'fANCE INDEX 

PO IS THE INVERSE OF P FROM THF PERFORMANCE INDEX 

RO IS THE BOUNDARY CONDITION FOR THE RICCATI EQUATION 

R IS THF SOLUTION OF THE MA'^P.IX RICCATI EQUATION 

KT IS THE GAINS '^ATRIX 

DT IS THE TI«E STEP 

NROW IS THE DIMENSION OF THE STATS VECTOR 
NCX IS THE NUMBER 0? COLUMNS IN THE 3 MATRIX 
IPNT IS THE PRINTER’S ACDRESS 

INPUT ARGUEMENTS AND RESULT 
REAL A(1) ,3 (1) ,QOr:) ,P0(1) ,R0(1) ,R(1) 

TEMPORARY STORAGE ARRAYS 

3PB,RA,AND ?G ARE GENERAL, ALL OTHERS ARE SYMMETRIC 
REAL BPS (10 0) ,RF(100) ,RD(100) ,PDT(100) ,R MAX (100) ,Q(100) 

1 ,BT (100) ,RA POO) ,RG (100) , KT ( NCX , N ROW) 

NLCOPS^O 
10 H=DT/6.0 

NQ=NROW* (NROW+1) /2 
TIME SCALE EQUATION 
CALCULATE ?=B*P*TRA(B) 

FIRST GET RK = P*TRA (B) 

DO 60 I=‘’,NCX 
DO 60 J=1,N?OW 
T=0. 0 

DO 50 K='*,NCX 
IA=I+ (K-1) *NCX 
IAT=K+ (1-1 ) *NCX 
IB=J+ (“"-I) *N ROW 
0 T=T+ (PO (lA) +PO (lAT) ) *3 (IB) 

IC = I4 (J-1) ^NCX 
0 RK(IC)=0.5*T 

THEN GET ?=3*PK 

EVEN THOUGH RESULT IS SYMMETRIC, DO FULL MATRIX FOR CONVENIENCE 
IC=0 

DO 80 J=1,NPOW 
DO 80 I=1,N?OW 
T=C. 0 

DO 70 K='>,NCX 
IA=I+ (K-1) *N?OW 
I3=K* (J-1 ) *NCX 
’’■=1+3 (lA) *R'^ (IB) 



0 
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IC=TC+1 

) BPB(IC)=H*T 

CONVERT F AND Q TO SYMIETPIC FORM AND FINISH riMS SCALING 
IC=0 

DO 90 0=1 , N ROW 
DO 90 

IA = I+ (J- 1) «=N?OW 
IB=J+ (1-1) *MFOW 
IC=IC+1 

P (IC) =0. 5* (?0 (lA) +RO (IB) ) 

) Q(IC) =0. 5*H* (QO (lA) +QO (IB) ) 

TIME=0. 0 
WRITE (IPNI , 9000) 

)00 FORMAT ( ' O' , 13, ' I • , - 1 9/ ' DT • ,T28 , ' ERROR • ,137 , ' CONTROLLEP GAINS • ) 
DO 190 1=1, NQ 
>0 RM AX (I) =0.0 

RONGE-K'JTTA INTEGERATION 
>0 BO 2 50 K = '', 10 
DO 210 1=1, NQ 
lO BT(I)=R(I) 

CALL MR DOT ( A , BP B, Q, BT , R A , RG , R DT , H, NRO W ) 

DO 220 r=1,NQ 
RD (I) =RDT (I) 

!0 BT (I) =R (I) ♦■3. 0*RD (I) 

CALL MRDOT { A , BPB, Q , , P A , RG , P DT , H, NEOW ) 

DO 230 1=1, NQ 

BT (I) =a (I) <•?. 0*?DT (I) 

!0 RD (I)=RD(I) +2. 0*RDT(I) 

CALL MRDO"' ( A , E? 3 , Q , BT , E A , FG , RDT , H , NROW) 

DO 2U0 r=1,NQ 
BT (I) =R (I) +6. 0*RDT (I) 
iO RD (I) =RD (I) +2. 0*RDT (I) 

CALL MRDOT ( A , BP3 , Q , BT , ? A , RG , RDT , H , NRO W) 

TIME STEP IS COMPLETE 

UPDATE R AND COMPOTE ERROR NORM 

DRS=0.0 

DO 250 1=1, NQ 

DR = ED (I) *RDT (I) 

R (I) =R (I) +D? 

DR=ABS (DR/DT) 

RMAX (I) =AM AX'* (DR, RMAX (I) ) 

I? (RMAX (I) .NE.0.0) DRS = AMAX1 (DRS,DR/RMAX (I) ) 
iO CONTINUE 

COMPOTE AND PRINT CONTROLLER GAINS 
K=P*TR A (B) ’*? 

TIKE=TIME+10.0*DT 
DO 280 1=1, NCX 
DO 270 J=1,NECW 
T=0. 0 

DO 260 K=1 , NROW 
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IA=I+ (K-1) *NCX 

G^T ELE{K,J) FRO**, K SYH^iETEIC MATRIX 
IF (K. LE. J) I 3=J* (J - 1) /2 + F 
IF (K.GT. J) I3 = K* (K’-l ) Z2 4- J 
50 T=T + PF (l A) *R ( IB) 

KT (I, J) =I 
^0 PD(J)=? 

IF (I. EQ. 1 ) HE (IPNT, 50 0 1 ) TIMS , DT , DHS , (E D { J) ,J = 1 ,NEOtf) 
)01 F0FMAT(1H , I PG 11 . C , 1 0G 1 2 . U , / ( • » , 40X , 9G 1 2 . U ) ) 

IF (I. GT. 'll WRITE (IPNT, 5 002) (?D (J) ,J=1 ,NFOW) 

)02 FOFMATni! ,35X, 1?8Gl2.4,/(* ' , 4 OX , SG 1 2 . U ) ) 

10 CONTINUE 

TERMINATE IF NOT CONVERGING 

IF (TIME. IT.-'' 30.0) GO TO 5^2 
TEPMINA’^E I? DSS IS SMALL 
IF (DRS.GT.0.00 I) GO "0 200 
GO TO SW? 

>72 NLOOPS = NLOOP3-*- 

IF (NLOOPS. GT. 1) GO TO 573 
DT=DT/2. 

GO TO 10 
>73 RETURN 
END 
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SUPPOUTINE MPDOT ( A , E P B, Q , BT , R A , ?,G , RDT , H , NPOW) 

COPYRIGHT 1973 ALL BIGHTS RESERVED 

rniS SUBHOOTINE IS CALLED BY >1PS 
IMPLICIT IHTEGE?,^2 (I-N) 

REAL A (1) ,3?B (1) ,Q C ) ,FT (1) , R A C ) , ?G ( 1 ) , R DT ( 1 ) 

PDOT (I,L) = -R ^A(F,L) - A (K,I) *R (K,L) -Q (I,L) (I, J) *P(J, K) *5 (K,L) 

NOTE THAT 3 CONTAINS CURRENT VERSION OF R FOR RUNGE-KUTTA 
FOE R*A + TRA (A) *F. USE FORMULA: 

TEEM (I, L) =R (I, K) *A (K,L) +A (K, I) •R (K,L) 

FOP. R*P*R USE FORMULA: 

TSFM (I,L) =R (I, J) *? (J,K) *R (K,L) 

-COMPUTE R*A NAD R^TE A ( B) * IN V ( P) *8 = 8* P SINCE P PRESET 

IJ=0 

DO 50 J=1,NHOW 
DO 50 I=1,NPOW 
TA=0.0 
TB=0.0 

IK=I* (1-1) /2 
KJ= (J-1) *NROV 
DO UO K=1,NROW 
IF (K.LE. I) GO TO 20 
IK=IK+K-1 
GO TO 30 
) IK=IK+1 
) KJ=KJ+1 

TA=TA+BT (IK) *A (KJ) 

) T3=TB+BT (IK) *BP3 (KJ) 

IJ=IJ+1 
RG (I J) =TR 
) RA(IJ)=TA*H 

HOW COMPUTE NEW RDOT 

IJ=0 

DO 100 J=1,NROW 
IL= (J-1) *NROW 
LI=J-NROW 
DO 100 1=1 , J 

COMPUTE RG*R 

TA=0. 0 

KJ=J* (J-"') /2 
IK=I-NROW 
DO 90 K=1 , NROW 
IF (K.LE. J) GO TO 70 
KJ=KJ+K-1 
GO TO 80 
) KJ=KJ+1 
) IK=IK+N?OW 
) TA=T A+PG (IK) *BT (KJ) 



X3D0T 



ij=r j+1 
IL=IL+1 
LI=LI+NROW 

PDT (IJ) =TA-0 (IJ) “PA (IL) -RA (LI) 
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